First pass at pipeline implementation

camera.c

@@ -0,0 +1,665 @@
+#include "camera.h"
+
+#include <assert.h>
+#include <errno.h>
+#include <glib.h>
+#include <sys/ioctl.h>
+#include <sys/mman.h>
+
+#define MAX_VIDEO_BUFFERS 20
+
+static const char *pixel_format_names[MP_PIXEL_FMT_MAX] = {
+    "unsupported",
+    "BGGR8",
+    "GBRG8",
+    "GRBG8",
+    "RGGB8",
+};
+
+const char *mp_pixel_format_to_str(uint32_t pixel_format)
+{
+    g_return_val_if_fail(pixel_format < MP_PIXEL_FMT_MAX, "INVALID");
+    return pixel_format_names[pixel_format];
+}
+
+MPPixelFormat mp_pixel_format_from_str(const char *name)
+{
+    for (MPPixelFormat i = 0; i < MP_PIXEL_FMT_MAX; ++i) {
+        if (strcasecmp(pixel_format_names[i], name) == 0) {
+            return i;
+        }
+    }
+    g_return_val_if_reached(MP_PIXEL_FMT_UNSUPPORTED);
+}
+
+static const uint32_t pixel_format_v4l_pixel_formats[MP_PIXEL_FMT_MAX] = {
+    0,
+    V4L2_PIX_FMT_SBGGR8,
+    V4L2_PIX_FMT_SGBRG8,
+    V4L2_PIX_FMT_SGRBG8,
+    V4L2_PIX_FMT_SRGGB8,
+};
+
+uint32_t mp_pixel_format_to_v4l_pixel_format(MPPixelFormat pixel_format)
+{
+    g_return_val_if_fail(pixel_format < MP_PIXEL_FMT_MAX, 0);
+    return pixel_format_v4l_pixel_formats[pixel_format];
+}
+
+MPPixelFormat mp_pixel_format_from_v4l_pixel_format(uint32_t v4l_pixel_format)
+{
+    for (MPPixelFormat i = 0; i < MP_PIXEL_FMT_MAX; ++i) {
+        if (pixel_format_v4l_pixel_formats[i] == v4l_pixel_format) {
+            return i;
+        }
+    }
+    return MP_PIXEL_FMT_UNSUPPORTED;
+}
+
+static const uint32_t pixel_format_v4l_bus_codes[MP_PIXEL_FMT_MAX] = {
+    0,
+    MEDIA_BUS_FMT_SBGGR8_1X8,
+    MEDIA_BUS_FMT_SGBRG8_1X8,
+    MEDIA_BUS_FMT_SGRBG8_1X8,
+    MEDIA_BUS_FMT_SRGGB8_1X8,
+};
+
+uint32_t mp_pixel_format_to_v4l_bus_code(MPPixelFormat pixel_format)
+{
+    g_return_val_if_fail(pixel_format < MP_PIXEL_FMT_MAX, 0);
+    return pixel_format_v4l_bus_codes[pixel_format];
+}
+
+MPPixelFormat mp_pixel_format_from_v4l_bus_code(uint32_t v4l_bus_code)
+{
+    for (MPPixelFormat i = 0; i < MP_PIXEL_FMT_MAX; ++i) {
+        if (pixel_format_v4l_bus_codes[i] == v4l_bus_code) {
+            return i;
+        }
+    }
+    return MP_PIXEL_FMT_UNSUPPORTED;
+}
+
+uint32_t mp_pixel_format_bytes_per_pixel(MPPixelFormat pixel_format)
+{
+    g_return_val_if_fail(pixel_format < MP_PIXEL_FMT_MAX, 0);
+    switch (pixel_format) {
+        case MP_PIXEL_FMT_BGGR8:
+        case MP_PIXEL_FMT_GBRG8:
+        case MP_PIXEL_FMT_GRBG8:
+        case MP_PIXEL_FMT_RGGB8: return 1;
+        default: return 0;
+    }
+}
+
+bool mp_camera_mode_is_equivalent(const MPCameraMode *m1, const MPCameraMode *m2)
+{
+    return m1->pixel_format == m2->pixel_format
+        && m1->frame_interval.numerator == m2->frame_interval.numerator
+        && m1->frame_interval.denominator == m2->frame_interval.denominator
+        && m1->width == m2->width
+        && m1->height == m2->height;
+}
+
+struct video_buffer {
+    uint32_t length;
+    uint8_t *data;
+};
+
+struct _MPCamera {
+    int video_fd;
+    int subdev_fd;
+
+    bool has_set_mode;
+    MPCameraMode current_mode;
+
+    struct video_buffer buffers[MAX_VIDEO_BUFFERS];
+    uint32_t num_buffers;
+};
+
+MPCamera *mp_camera_new(int video_fd, int subdev_fd)
+{
+    g_return_val_if_fail(video_fd != -1, NULL);
+
+    MPCamera *camera = malloc(sizeof(MPCamera));
+    camera->video_fd = video_fd;
+    camera->subdev_fd = subdev_fd;
+    camera->has_set_mode = false;
+    camera->num_buffers = 0;
+    return camera;
+}
+
+void mp_camera_free(MPCamera *camera)
+{
+    g_warn_if_fail(camera->num_buffers == 0);
+    if (camera->num_buffers != 0) {
+        mp_camera_stop_capture(camera);
+    }
+
+    free(camera);
+}
+
+bool mp_camera_is_subdev(MPCamera *camera)
+{
+    return camera->subdev_fd != -1;
+}
+
+int mp_camera_get_video_fd(MPCamera *camera)
+{
+    return camera->video_fd;
+}
+
+int mp_camera_get_subdev_fd(MPCamera *camera)
+{
+    return camera->subdev_fd;
+}
+
+static void errno_printerr(const char *s)
+{
+    g_printerr("MPCamera: %s error %d, %s\n", s, errno, strerror(errno));
+}
+
+static int xioctl(int fd, int request, void *arg)
+{
+    int r;
+    do {
+        r = ioctl(fd, request, arg);
+    } while (r == -1 && errno == EINTR);
+    return r;
+}
+
+bool mp_camera_try_mode(MPCamera *camera, MPCameraMode *mode)
+{
+    struct v4l2_format fmt = {};
+    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    fmt.fmt.pix.width = mode->width;
+    fmt.fmt.pix.height = mode->height;
+    fmt.fmt.pix.pixelformat = mp_pixel_format_from_v4l_pixel_format(mode->pixel_format);
+    fmt.fmt.pix.field = V4L2_FIELD_ANY;
+    if (xioctl(camera->video_fd, VIDIOC_TRY_FMT, &fmt) == -1) {
+        errno_printerr("VIDIOC_S_FMT");
+        return false;
+    }
+
+    mode->width = fmt.fmt.pix.width;
+    mode->height = fmt.fmt.pix.height;
+    mode->pixel_format = mp_pixel_format_from_v4l_pixel_format(fmt.fmt.pix.pixelformat);
+
+    return true;
+}
+
+const MPCameraMode *mp_camera_get_mode(const MPCamera *camera)
+{
+    return &camera->current_mode;
+}
+
+bool mp_camera_set_mode(MPCamera *camera, MPCameraMode *mode)
+{
+    // Set the mode in the subdev the camera is one
+    if (mp_camera_is_subdev(camera))
+    {
+        struct v4l2_subdev_frame_interval interval = {};
+        interval.pad = 0;
+        interval.interval = mode->frame_interval;
+        if (xioctl(camera->subdev_fd, VIDIOC_SUBDEV_S_FRAME_INTERVAL, &interval) == -1) {
+            errno_printerr("VIDIOC_SUBDEV_S_FRAME_INTERVAL");
+            return false;
+        }
+
+        bool did_set_frame_rate =
+            interval.interval.numerator == mode->frame_interval.numerator
+            && interval.interval.denominator == mode->frame_interval.denominator;
+
+        struct v4l2_subdev_format fmt = {};
+        fmt.pad = 0;
+        fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+        fmt.format.width = mode->width;
+        fmt.format.height = mode->height;
+        fmt.format.code = mp_pixel_format_to_v4l_bus_code(mode->pixel_format);
+        fmt.format.field = V4L2_FIELD_ANY;
+        if (xioctl(camera->subdev_fd, VIDIOC_SUBDEV_S_FMT, &fmt) == -1) {
+            errno_printerr("VIDIOC_SUBDEV_S_FMT");
+            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 didn't
+        // succeed before. Ideally we'd be able to set both at once.
+        if (!did_set_frame_rate)
+        {
+            interval.interval = mode->frame_interval;
+            if (xioctl(camera->subdev_fd, VIDIOC_SUBDEV_S_FRAME_INTERVAL, &interval) == -1) {
+                errno_printerr("VIDIOC_SUBDEV_S_FRAME_INTERVAL");
+                return false;
+            }
+        }
+
+        // Update the mode
+        mode->pixel_format = mp_pixel_format_from_v4l_bus_code(fmt.format.code);
+        mode->frame_interval = interval.interval;
+        mode->width = fmt.format.width;
+        mode->height = fmt.format.height;
+    }
+
+    // Set the mode for the video device
+    {
+        struct v4l2_format fmt = {};
+        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        fmt.fmt.pix.width = mode->width;
+        fmt.fmt.pix.height = mode->height;
+        fmt.fmt.pix.pixelformat = mp_pixel_format_to_v4l_pixel_format(mode->pixel_format);
+        fmt.fmt.pix.field = V4L2_FIELD_ANY;
+        if (xioctl(camera->video_fd, VIDIOC_S_FMT, &fmt) == -1) {
+            errno_printerr("VIDIOC_S_FMT");
+            return false;
+        }
+
+        // Update the mode
+        mode->pixel_format = mp_pixel_format_from_v4l_pixel_format(fmt.fmt.pix.pixelformat);
+        mode->width = fmt.fmt.pix.width;
+        mode->height = fmt.fmt.pix.height;
+    }
+
+    camera->has_set_mode = true;
+    camera->current_mode = *mode;
+
+    return true;
+}
+
+bool mp_camera_start_capture(MPCamera *camera)
+{
+    g_return_val_if_fail(camera->has_set_mode, false);
+    g_return_val_if_fail(camera->num_buffers == 0, false);
+
+    // Start by requesting buffers
+    struct v4l2_requestbuffers req = {};
+    req.count = MAX_VIDEO_BUFFERS;
+    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    req.memory = V4L2_MEMORY_MMAP;
+
+    if (xioctl(camera->video_fd, VIDIOC_REQBUFS, &req) == -1) {
+        errno_printerr("VIDIOC_REQBUFS");
+        return false;
+    }
+
+    if (req.count < 2) {
+        g_printerr("Insufficient buffer memory. Only %d buffers available.\n",
+                   req.count);
+        goto error;
+    }
+
+    for (uint32_t i = 0; i < req.count; ++i) {
+        // Query each buffer and mmap it
+        struct v4l2_buffer buf = {
+            .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+            .memory = V4L2_MEMORY_MMAP,
+            .index = i,
+        };
+
+        if (xioctl(camera->video_fd, VIDIOC_QUERYBUF, &buf) == -1) {
+            errno_printerr("VIDIOC_QUERYBUF");
+            break;
+        }
+
+        camera->buffers[i].length = buf.length;
+        camera->buffers[i].data = mmap(
+            NULL,
+            buf.length,
+            PROT_READ,
+            MAP_SHARED,
+            camera->video_fd,
+            buf.m.offset);
+
+        if (camera->buffers[i].data == MAP_FAILED) {
+            errno_printerr("mmap");
+            break;
+        }
+
+        ++camera->num_buffers;
+    }
+
+    if (camera->num_buffers != req.count) {
+        g_printerr("Unable to map all buffers\n");
+        goto error;
+    }
+
+    for (uint32_t i = 0; i < camera->num_buffers; ++i) {
+        struct v4l2_buffer buf = {
+            .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+            .memory = V4L2_MEMORY_MMAP,
+            .index = i,
+        };
+
+        // Queue the buffer for capture
+        if (xioctl(camera->video_fd, VIDIOC_QBUF, &buf) == -1) {
+            errno_printerr("VIDIOC_QBUF");
+            goto error;
+        }
+    }
+
+    // Start capture
+    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    if (xioctl(camera->video_fd, VIDIOC_STREAMON, &type) == -1) {
+        errno_printerr("VIDIOC_STREAMON");
+        goto error;
+    }
+
+    return true;
+
+error:
+    // Unmap any mapped buffers
+    assert(camera->num_buffers <= MAX_VIDEO_BUFFERS);
+    for (uint32_t i = 0; i < camera->num_buffers; ++i) {
+        if (munmap(camera->buffers[i].data, camera->buffers[i].length) == -1) {
+            errno_printerr("munmap");
+        }
+    }
+
+    // Reset allocated buffers
+    {
+        struct v4l2_requestbuffers req = {};
+        req.count = 0;
+        req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        req.memory = V4L2_MEMORY_MMAP;
+
+        if (xioctl(camera->video_fd, VIDIOC_REQBUFS, &req) == -1) {
+            errno_printerr("VIDIOC_REQBUFS");
+        }
+    }
+
+    return false;
+}
+
+bool mp_camera_stop_capture(MPCamera *camera)
+{
+    g_return_val_if_fail(camera->num_buffers > 0, false);
+
+    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    if (xioctl(camera->video_fd, VIDIOC_STREAMOFF, &type) == -1) {
+        errno_printerr("VIDIOC_STREAMOFF");
+    }
+
+    assert(camera->num_buffers <= MAX_VIDEO_BUFFERS);
+    for (int i = 0; i < camera->num_buffers; ++i) {
+        if (munmap(camera->buffers[i].data, camera->buffers[i].length) == -1) {
+            errno_printerr("munmap");
+        }
+    }
+
+    camera->num_buffers = 0;
+
+    struct v4l2_requestbuffers req = {};
+    req.count = 0;
+    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    req.memory = V4L2_MEMORY_MMAP;
+    if (xioctl(camera->video_fd, VIDIOC_REQBUFS, &req) == -1) {
+        errno_printerr("VIDIOC_REQBUFS");
+    }
+
+    return true;
+}
+
+bool mp_camera_is_capturing(MPCamera *camera)
+{
+    return camera->num_buffers > 0;
+}
+
+bool mp_camera_capture_image(MPCamera *camera, void (*callback)(MPImage, void *), void *user_data)
+{
+    struct v4l2_buffer buf = {};
+    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    buf.memory = V4L2_MEMORY_MMAP;
+    if (xioctl(camera->video_fd, VIDIOC_DQBUF, &buf) == -1) {
+        switch (errno) {
+            case EAGAIN:
+                return true;
+            case EIO:
+                /* Could ignore EIO, see spec. */
+                /* fallthrough */
+            default:
+                errno_printerr("VIDIOC_DQBUF");
+                return false;
+        }
+    }
+
+    uint32_t pixel_format = camera->current_mode.pixel_format;
+    uint32_t width = camera->current_mode.width;
+    uint32_t height = camera->current_mode.height;
+
+    assert(buf.bytesused == mp_pixel_format_bytes_per_pixel(pixel_format) * width * height);
+    assert(buf.bytesused == camera->buffers[buf.index].length);
+
+    MPImage image = {
+        .pixel_format = pixel_format,
+        .width = width,
+        .height = height,
+        .data = camera->buffers[buf.index].data,
+    };
+
+    callback(image, user_data);
+
+    // The callback may have stopped the capture, only queue the buffer if we're
+    // still capturing.
+    if (mp_camera_is_capturing(camera)) {
+        if (xioctl(camera->video_fd, VIDIOC_QBUF, &buf) == -1) {
+            errno_printerr("VIDIOC_QBUF");
+            return false;
+        }
+    }
+
+    return true;
+}
+
+struct _MPCameraModeList {
+    MPCameraMode mode;
+    MPCameraModeList *next;
+};
+
+static MPCameraModeList *
+get_subdev_modes(MPCamera *camera, bool (*check)(MPCamera *, MPCameraMode *))
+{
+    MPCameraModeList *item = NULL;
+
+    for (uint32_t fmt_index = 0;; ++fmt_index) {
+        struct v4l2_subdev_mbus_code_enum fmt = {};
+        fmt.index = fmt_index;
+        fmt.pad = 0;
+        fmt.which = V4L2_SUBDEV_FORMAT_TRY;
+        if (xioctl(camera->subdev_fd, VIDIOC_SUBDEV_ENUM_MBUS_CODE, &fmt) == -1) {
+            if (errno != EINVAL) {
+                errno_printerr("VIDIOC_SUBDEV_ENUM_MBUS_CODE");
+            }
+            break;
+        }
+
+        // Skip unsupported formats
+        uint32_t format = mp_pixel_format_from_v4l_bus_code(fmt.code);
+        if (format == MP_PIXEL_FMT_UNSUPPORTED) {
+            continue;
+        }
+
+        for (uint32_t frame_index = 0;; ++frame_index) {
+            struct v4l2_subdev_frame_size_enum frame = {};
+            frame.index = frame_index;
+            frame.pad = 0;
+            frame.code = fmt.code;
+            frame.which = V4L2_SUBDEV_FORMAT_TRY;
+            if (xioctl(camera->subdev_fd, VIDIOC_SUBDEV_ENUM_FRAME_SIZE, &frame) == -1) {
+                if (errno != EINVAL) {
+                    errno_printerr("VIDIOC_SUBDEV_ENUM_FRAME_SIZE");
+                }
+                break;
+            }
+
+            // TODO: Handle other types
+            if (frame.min_width != frame.max_width
+                || frame.min_height != frame.max_height) {
+                break;
+            }
+
+            for (uint32_t interval_index = 0;; ++interval_index) {
+                struct v4l2_subdev_frame_interval_enum interval = {};
+                interval.index = interval_index;
+                interval.pad = 0;
+                interval.code = fmt.code;
+                interval.width = frame.max_width;
+                interval.height = frame.max_height;
+                interval.which = V4L2_SUBDEV_FORMAT_TRY;
+                if (xioctl(camera->subdev_fd, VIDIOC_SUBDEV_ENUM_FRAME_INTERVAL, &interval) == -1) {
+                    if (errno != EINVAL) {
+                        errno_printerr("VIDIOC_SUBDEV_ENUM_FRAME_INTERVAL");
+                    }
+                    break;
+                }
+
+                MPCameraMode mode = {
+                    .pixel_format = format,
+                    .frame_interval = interval.interval,
+                    .width = frame.max_width,
+                    .height = frame.max_height,
+                };
+
+                if (!check(camera, &mode)) {
+                    continue;
+                }
+
+                MPCameraModeList *new_item = malloc(sizeof(MPCameraModeList));
+                new_item->mode = mode;
+                new_item->next = item;
+                item = new_item;
+            }
+        }
+    }
+
+    return item;
+}
+
+static MPCameraModeList *
+get_video_modes(MPCamera *camera, bool (*check)(MPCamera *, MPCameraMode *))
+{
+    MPCameraModeList *item = NULL;
+
+    for (uint32_t fmt_index = 0;; ++fmt_index) {
+        struct v4l2_fmtdesc fmt = {};
+        fmt.index = fmt_index;
+        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        if (xioctl(camera->video_fd, VIDIOC_ENUM_FMT, &fmt) == -1) {
+            if (errno != EINVAL) {
+                errno_printerr("VIDIOC_ENUM_FMT");
+            }
+            break;
+        }
+
+        // Skip unsupported formats
+        uint32_t format = mp_pixel_format_from_v4l_pixel_format(fmt.pixelformat);
+        if (format == MP_PIXEL_FMT_UNSUPPORTED) {
+            continue;
+        }
+
+        for (uint32_t frame_index = 0;; ++frame_index) {
+            struct v4l2_frmsizeenum frame = {};
+            frame.index = frame_index;
+            frame.pixel_format = fmt.pixelformat;
+            if (xioctl(camera->video_fd, VIDIOC_ENUM_FRAMESIZES, &frame) == -1) {
+                if (errno != EINVAL) {
+                    errno_printerr("VIDIOC_ENUM_FRAMESIZES");
+                }
+                break;
+            }
+
+            // TODO: Handle other types
+            if (frame.type != V4L2_FRMSIZE_TYPE_DISCRETE) {
+                break;
+            }
+
+            for (uint32_t interval_index = 0;; ++interval_index) {
+                struct v4l2_frmivalenum interval = {};
+                interval.index = interval_index;
+                interval.pixel_format = fmt.pixelformat;
+                interval.width = frame.discrete.width;
+                interval.height = frame.discrete.height;
+                if (xioctl(camera->video_fd, VIDIOC_ENUM_FRAMEINTERVALS, &interval) == -1) {
+                    if (errno != EINVAL) {
+                        errno_printerr("VIDIOC_ENUM_FRAMESIZES");
+                    }
+                    break;
+                }
+
+                // TODO: Handle other types
+                if (interval.type != V4L2_FRMIVAL_TYPE_DISCRETE) {
+                    break;
+                }
+
+                MPCameraMode mode = {
+                    .pixel_format = format,
+                    .frame_interval = interval.discrete,
+                    .width = frame.discrete.width,
+                    .height = frame.discrete.height,
+                };
+
+                if (!check(camera, &mode)) {
+                    continue;
+                }
+
+                MPCameraModeList *new_item = malloc(sizeof(MPCameraModeList));
+                new_item->mode = mode;
+                new_item->next = item;
+                item = new_item;
+            }
+        }
+    }
+
+    return item;
+}
+
+static bool all_modes(MPCamera *camera, MPCameraMode *mode)
+{
+    return true;
+}
+
+static bool available_modes(MPCamera *camera, MPCameraMode *mode)
+{
+    MPCameraMode attempt = *mode;
+    return mp_camera_try_mode(camera, &attempt)
+        && mp_camera_mode_is_equivalent(mode, &attempt);
+}
+
+MPCameraModeList *mp_camera_list_supported_modes(MPCamera *camera)
+{
+    if (mp_camera_is_subdev(camera)) {
+        return get_subdev_modes(camera, all_modes);
+    } else {
+        return get_video_modes(camera, all_modes);
+    }
+}
+
+MPCameraModeList *mp_camera_list_available_modes(MPCamera *camera)
+{
+    if (mp_camera_is_subdev(camera)) {
+        return get_subdev_modes(camera, available_modes);
+    } else {
+        return get_video_modes(camera, available_modes);
+    }
+}
+
+MPCameraMode *mp_camera_mode_list_get(MPCameraModeList *list)
+{
+    g_return_val_if_fail(list, NULL);
+    return &list->mode;
+}
+
+MPCameraModeList *mp_camera_mode_list_next(MPCameraModeList *list)
+{
+    g_return_val_if_fail(list, NULL);
+    return list->next;
+}
+
+void mp_camera_mode_list_free(MPCameraModeList *list)
+{
+    while (list) {
+        MPCameraModeList *tmp = list;
+        list = tmp->next;
+        free(tmp);
+    }
+}

camera.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#include <linux/v4l2-subdev.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef enum {
+    MP_PIXEL_FMT_UNSUPPORTED,
+    MP_PIXEL_FMT_BGGR8,
+    MP_PIXEL_FMT_GBRG8,
+    MP_PIXEL_FMT_GRBG8,
+    MP_PIXEL_FMT_RGGB8,
+
+    MP_PIXEL_FMT_MAX,
+} MPPixelFormat;
+
+MPPixelFormat mp_pixel_format_from_str(const char *str);
+const char *mp_pixel_format_to_str(MPPixelFormat pixel_format);
+
+MPPixelFormat mp_pixel_format_from_v4l_pixel_format(uint32_t v4l_pixel_format);
+MPPixelFormat mp_pixel_format_from_v4l_bus_code(uint32_t v4l_bus_code);
+uint32_t mp_pixel_format_to_v4l_pixel_format(MPPixelFormat pixel_format);
+uint32_t mp_pixel_format_to_v4l_bus_code(MPPixelFormat pixel_format);
+
+uint32_t mp_pixel_format_bytes_per_pixel(MPPixelFormat pixel_format);
+
+typedef struct {
+    MPPixelFormat pixel_format;
+
+    struct v4l2_fract frame_interval;
+    uint32_t width;
+    uint32_t height;
+} MPCameraMode;
+
+bool mp_camera_mode_is_equivalent(const MPCameraMode *m1, const MPCameraMode *m2);
+
+typedef struct {
+    uint32_t pixel_format;
+    uint32_t width;
+    uint32_t height;
+    uint8_t *data;
+} MPImage;
+
+typedef struct _MPCamera MPCamera;
+
+MPCamera *mp_camera_new(int video_fd, int subdev_fd);
+void mp_camera_free(MPCamera *camera);
+
+bool mp_camera_is_subdev(MPCamera *camera);
+int mp_camera_get_video_fd(MPCamera *camera);
+int mp_camera_get_subdev_fd(MPCamera *camera);
+
+const MPCameraMode *mp_camera_get_mode(const MPCamera *camera);
+bool mp_camera_try_mode(MPCamera *camera, MPCameraMode *mode);
+
+bool mp_camera_set_mode(MPCamera *camera, MPCameraMode *mode);
+bool mp_camera_start_capture(MPCamera *camera);
+bool mp_camera_stop_capture(MPCamera *camera);
+bool mp_camera_is_capturing(MPCamera *camera);
+bool mp_camera_capture_image(MPCamera *camera, void (*callback)(MPImage, void *), void *user_data);
+
+typedef struct _MPCameraModeList MPCameraModeList;
+
+MPCameraModeList *mp_camera_list_supported_modes(MPCamera *camera);
+MPCameraModeList *mp_camera_list_available_modes(MPCamera *camera);
+MPCameraMode *mp_camera_mode_list_get(MPCameraModeList *list);
+MPCameraModeList *mp_camera_mode_list_next(MPCameraModeList *list);
+void mp_camera_mode_list_free(MPCameraModeList *list);

device.c

@@ -0,0 +1,402 @@
+#include "device.h"
+
+#include <errno.h>
+#include <fcntl.h>
+#include <glib.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+
+bool mp_find_device_path(struct media_v2_intf_devnode devnode, char *path, int length)
+{
+    char uevent_path[256];
+    snprintf(uevent_path, 256, "/sys/dev/char/%d:%d/uevent", devnode.major, devnode.minor);
+
+    FILE *f = fopen(uevent_path, "r");
+    if (!f) {
+        return false;
+    }
+
+    char line[512];
+    while (fgets(line, 512, f)) {
+        if (strncmp(line, "DEVNAME=", 8) == 0) {
+            // Drop newline
+            int length = strlen(line);
+            if (line[length - 1] == '\n')
+                line[length - 1] = '\0';
+
+            snprintf(path, length, "/dev/%s", line + 8);
+            return true;
+        }
+    }
+
+    fclose(f);
+
+    return false;
+}
+
+struct _MPDevice {
+    int fd;
+
+    struct media_device_info info;
+
+    struct media_v2_entity *entities;
+    size_t num_entities;
+    struct media_v2_interface *interfaces;
+    size_t num_interfaces;
+    struct media_v2_pad *pads;
+    size_t num_pads;
+    struct media_v2_link *links;
+    size_t num_links;
+};
+
+static void errno_printerr(const char *s)
+{
+    g_printerr("MPDevice: %s error %d, %s\n", s, errno, strerror(errno));
+}
+
+static int xioctl(int fd, int request, void *arg)
+{
+    int r;
+    do {
+        r = ioctl(fd, request, arg);
+    } while (r == -1 && errno == EINTR);
+    return r;
+}
+
+MPDevice *mp_device_find(const char *driver_name)
+{
+    MPDevice *found_device = NULL;
+
+    int length = strlen(driver_name);
+    MPDeviceList *list = mp_device_list_new();
+
+    for (MPDeviceList *item = list; item; item = mp_device_list_next(item)) {
+        MPDevice *device = mp_device_list_get(item);
+        const struct media_device_info *info = mp_device_get_info(device);
+
+        if (strncmp(info->driver, driver_name, length) == 0) {
+            found_device = mp_device_list_remove(&item);
+            break;
+        }
+    }
+
+    mp_device_list_free(list);
+
+    return found_device;
+}
+
+MPDevice *mp_device_open(const char *path)
+{
+    int fd = open(path, O_RDWR);
+    if (fd == -1) {
+        errno_printerr("open");
+        return NULL;
+    }
+
+    return mp_device_new(fd);
+}
+
+MPDevice *mp_device_new(int fd)
+{
+    // Get the topology of the media device
+    struct media_v2_topology topology = {};
+    if (xioctl(fd, MEDIA_IOC_G_TOPOLOGY, &topology) == -1
+        || topology.num_entities == 0) {
+        close(fd);
+        return NULL;
+    }
+
+    // Create the device
+    MPDevice *device = calloc(1, sizeof(MPDevice));
+    device->fd = fd;
+    device->entities = calloc(topology.num_entities, sizeof(struct media_v2_entity));
+    device->num_entities = topology.num_entities;
+    device->interfaces = calloc(topology.num_interfaces, sizeof(struct media_v2_interface));
+    device->num_interfaces = topology.num_interfaces;
+    device->pads = calloc(topology.num_pads, sizeof(struct media_v2_pad));
+    device->num_pads = topology.num_pads;
+    device->links = calloc(topology.num_links, sizeof(struct media_v2_link));
+    device->num_links = topology.num_links;
+
+    // Get the actual devices and interfaces
+    topology.ptr_entities = (uint64_t)device->entities;
+    topology.ptr_interfaces = (uint64_t)device->interfaces;
+    topology.ptr_pads = (uint64_t)device->pads;
+    topology.ptr_links = (uint64_t)device->links;
+    if (xioctl(fd, MEDIA_IOC_G_TOPOLOGY, &topology) == -1) {
+        errno_printerr("MEDIA_IOC_G_TOPOLOGY");
+        mp_device_close(device);
+        return NULL;
+    }
+
+    // Get device info
+    if (xioctl(fd, MEDIA_IOC_DEVICE_INFO, &device->info) == -1) {
+        errno_printerr("MEDIA_IOC_DEVICE_INFO");
+        mp_device_close(device);
+        return NULL;
+    }
+
+    return device;
+}
+
+void mp_device_close(MPDevice *device)
+{
+    close(device->fd);
+    free(device->entities);
+    free(device->interfaces);
+    free(device->pads);
+    free(device->links);
+    free(device);
+}
+
+bool mp_device_setup_link(MPDevice *device, uint32_t source_pad_id, uint32_t sink_pad_id, bool enabled)
+{
+    const struct media_v2_pad *source_pad = mp_device_get_pad(device, source_pad_id);
+    g_return_val_if_fail(source_pad, false);
+
+    const struct media_v2_pad *sink_pad = mp_device_get_pad(device, sink_pad_id);
+    g_return_val_if_fail(sink_pad, false);
+
+    struct media_link_desc link = {};
+    link.flags = enabled ? MEDIA_LNK_FL_ENABLED : 0;
+    link.source.entity = source_pad->entity_id;
+    link.source.index = 0;
+    link.sink.entity = sink_pad->entity_id;
+    link.sink.index = 0;
+    if (xioctl(device->fd, MEDIA_IOC_SETUP_LINK, &link) == -1) {
+        errno_printerr("MEDIA_IOC_SETUP_LINK");
+        return false;
+    }
+
+    return true;
+}
+
+const struct media_v2_entity *mp_device_find_entity(const MPDevice *device, const char *driver_name)
+{
+    int length = strlen(driver_name);
+
+    // Find the entity from the name
+    for (uint32_t i = 0; i < device->num_entities; ++i) {
+        if (strncmp(device->entities[i].name, driver_name, length) == 0) {
+            return &device->entities[i];
+        }
+    }
+    return NULL;
+}
+
+const struct media_device_info *mp_device_get_info(const MPDevice *device)
+{
+    return &device->info;
+}
+
+const struct media_v2_entity *mp_device_get_entity(const MPDevice *device, uint32_t id)
+{
+    for (int i = 0; i < device->num_entities; ++i) {
+        if (device->entities[i].id == id) {
+            return &device->entities[i];
+        }
+    }
+    return NULL;
+}
+
+const struct media_v2_entity *mp_device_get_entities(const MPDevice *device)
+{
+    return device->entities;
+}
+
+size_t mp_device_get_num_entities(const MPDevice *device)
+{
+    return device->num_entities;
+}
+
+const struct media_v2_interface *mp_device_find_entity_interface(const MPDevice *device, uint32_t entity_id)
+{
+    // Find the interface through the link
+    const struct media_v2_link *link = mp_device_find_link_to(device, entity_id);
+    if (!link) {
+        return NULL;
+    }
+    return mp_device_get_interface(device, link->source_id);
+}
+
+const struct media_v2_interface *mp_device_get_interface(const MPDevice *device, uint32_t id)
+{
+    for (int i = 0; i < device->num_interfaces; ++i) {
+        if (device->interfaces[i].id == id) {
+            return &device->interfaces[i];
+        }
+    }
+    return NULL;
+}
+
+const struct media_v2_interface *mp_device_get_interfaces(const MPDevice *device)
+{
+    return device->interfaces;
+}
+
+size_t mp_device_get_num_interfaces(const MPDevice *device)
+{
+    return device->num_interfaces;
+}
+
+const struct media_v2_pad *mp_device_get_pad_from_entity(const MPDevice *device, uint32_t entity_id)
+{
+    for (int i = 0; i < device->num_pads; ++i) {
+        if (device->pads[i].entity_id == entity_id) {
+            return &device->pads[i];
+        }
+    }
+    return NULL;
+}
+
+const struct media_v2_pad *mp_device_get_pad(const MPDevice *device, uint32_t id)
+{
+    for (int i = 0; i < device->num_pads; ++i) {
+        if (device->pads[i].id == id) {
+            return &device->pads[i];
+        }
+    }
+    return NULL;
+}
+
+const struct media_v2_pad *mp_device_get_pads(const MPDevice *device)
+{
+    return device->pads;
+}
+
+size_t mp_device_get_num_pads(const MPDevice *device)
+{
+    return device->num_pads;
+}
+
+const struct media_v2_link *mp_device_find_entity_link(const MPDevice *device, uint32_t entity_id)
+{
+    const struct media_v2_pad *pad = mp_device_get_pad_from_entity(device, entity_id);
+    const struct media_v2_link *link = mp_device_find_link_to(device, pad->id);
+    if (link) {
+        return link;
+    }
+    return mp_device_find_link_from(device, pad->id);
+}
+
+const struct media_v2_link *mp_device_find_link_from(const MPDevice *device, uint32_t source)
+{
+    for (int i = 0; i < device->num_links; ++i) {
+        if (device->links[i].source_id == source) {
+            return &device->links[i];
+        }
+    }
+    return NULL;
+}
+
+const struct media_v2_link *mp_device_find_link_to(const MPDevice *device, uint32_t sink)
+{
+    for (int i = 0; i < device->num_links; ++i) {
+        if (device->links[i].sink_id == sink) {
+            return &device->links[i];
+        }
+    }
+    return NULL;
+}
+
+const struct media_v2_link *mp_device_find_link_between(const MPDevice *device, uint32_t source, uint32_t sink)
+{
+    for (int i = 0; i < device->num_links; ++i) {
+        if (device->links[i].source_id == source
+            && device->links[i].sink_id == sink) {
+            return &device->links[i];
+        }
+    }
+    return NULL;
+}
+
+const struct media_v2_link *mp_device_get_link(const MPDevice *device, uint32_t id)
+{
+    for (int i = 0; i < device->num_links; ++i) {
+        if (device->links[i].id == id) {
+            return &device->links[i];
+        }
+    }
+    return NULL;
+}
+
+const struct media_v2_link *mp_device_get_links(const MPDevice *device)
+{
+    return device->links;
+}
+
+size_t mp_device_get_num_links(const MPDevice *device)
+{
+    return device->num_links;
+}
+
+struct _MPDeviceList {
+    MPDevice *device;
+    MPDeviceList *next;
+};
+
+MPDeviceList *mp_device_list_new()
+{
+    MPDeviceList *current = NULL;
+
+    // Enumerate media device files
+    struct dirent *dir;
+    DIR *d = opendir("/dev");
+    while ((dir = readdir(d)) != NULL) {
+        if (strncmp(dir->d_name, "media", 5) == 0) {
+            char path[261];
+            snprintf(path, 261, "/dev/%s", dir->d_name);
+
+            MPDevice *device = mp_device_open(path);
+
+            if (device) {
+                MPDeviceList *next = malloc(sizeof(MPDeviceList));
+                next->device = device;
+                next->next = current;
+                current = next;
+            }
+        }
+    }
+    closedir(d);
+
+    return current;
+}
+
+void mp_device_list_free(MPDeviceList *device_list)
+{
+    while (device_list) {
+        MPDeviceList *tmp = device_list;
+        device_list = tmp->next;
+
+        mp_device_close(tmp->device);
+        free(tmp);
+    }
+}
+
+MPDevice *mp_device_list_remove(MPDeviceList **device_list)
+{
+    MPDevice *device = (*device_list)->device;
+
+    if ((*device_list)->next) {
+        MPDeviceList *tmp = (*device_list)->next;
+        **device_list = *tmp;
+        free(tmp);
+    } else {
+        free(*device_list);
+        *device_list = NULL;
+    }
+
+    return device;
+}
+
+MPDevice *mp_device_list_get(const MPDeviceList *device_list)
+{
+    return device_list->device;
+}
+
+MPDeviceList *mp_device_list_next(const MPDeviceList *device_list)
+{
+    return device_list->next;
+}

device.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <linux/media.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+bool mp_find_device_path(struct media_v2_intf_devnode devnode, char *path, int length);
+
+typedef struct _MPDevice MPDevice;
+
+MPDevice *mp_device_find(const char *driver_name);
+MPDevice *mp_device_open(const char *path);
+MPDevice *mp_device_new(int fd);
+void mp_device_close(MPDevice *device);
+
+bool mp_device_setup_link(MPDevice *device, uint32_t source_pad_id, uint32_t sink_pad_id, bool enabled);
+
+const struct media_device_info *mp_device_get_info(const MPDevice *device);
+const struct media_v2_entity *mp_device_find_entity(const MPDevice *device, const char *driver_name);
+const struct media_v2_entity *mp_device_get_entity(const MPDevice *device, uint32_t id);
+const struct media_v2_entity *mp_device_get_entities(const MPDevice *device);
+size_t mp_device_get_num_entities(const MPDevice *device);
+const struct media_v2_interface *mp_device_find_entity_interface(const MPDevice *device, uint32_t entity_id);
+const struct media_v2_interface *mp_device_get_interface(const MPDevice *device, uint32_t id);
+const struct media_v2_interface *mp_device_get_interfaces(const MPDevice *device);
+size_t mp_device_get_num_interfaces(const MPDevice *device);
+const struct media_v2_pad *mp_device_get_pad_from_entity(const MPDevice *device, uint32_t entity_id);
+const struct media_v2_pad *mp_device_get_pad(const MPDevice *device, uint32_t id);
+const struct media_v2_pad *mp_device_get_pads(const MPDevice *device);
+size_t mp_device_get_num_pads(const MPDevice *device);
+const struct media_v2_link *mp_device_find_entity_link(const MPDevice *device, uint32_t entity_id);
+const struct media_v2_link *mp_device_find_link_from(const MPDevice *device, uint32_t source);
+const struct media_v2_link *mp_device_find_link_to(const MPDevice *device, uint32_t sink);
+const struct media_v2_link *mp_device_find_link_between(const MPDevice *device, uint32_t source, uint32_t sink);
+const struct media_v2_link *mp_device_get_link(const MPDevice *device, uint32_t id);
+const struct media_v2_link *mp_device_get_links(const MPDevice *device);
+size_t mp_device_get_num_links(const MPDevice *device);
+
+typedef struct _MPDeviceList MPDeviceList;
+
+MPDeviceList *mp_device_list_new();
+void mp_device_list_free(MPDeviceList *device_list);
+
+MPDevice *mp_device_list_remove(MPDeviceList **device_list);
+
+MPDevice *mp_device_list_get(const MPDeviceList *device_list);
+MPDeviceList *mp_device_list_next(const MPDeviceList *device_list);

meson.build

@@ -2,6 +2,7 @@ project('megapixels', 'c')
 gnome = import('gnome')
 gtkdep = dependency('gtk+-3.0')
 tiff = dependency('libtiff-4')
+threads = dependency('threads')
 
 cc = meson.get_compiler('c')
 libm = cc.find_library('m', required: false)
@@ -15,7 +16,7 @@ configure_file(
   output: 'config.h',
   configuration: conf )
 
-executable('megapixels', 'main.c', 'ini.c', 'quickpreview.c', resources, dependencies : [gtkdep, libm, tiff], install : true)
+executable('megapixels', 'main.c', 'ini.c', 'quickpreview.c', 'camera.c', 'device.c', 'pipeline.c', resources, dependencies : [gtkdep, libm, tiff, threads], install : true)
 
 install_data(['data/org.postmarketos.Megapixels.desktop'],
              install_dir : get_option('datadir') / 'applications')
@@ -38,3 +39,6 @@ install_data([
 install_data(['postprocess.sh'],
   install_dir : get_option('datadir') / 'megapixels/',
   install_mode: 'rwxr-xr-x')
+
+executable('list_devices', 'tools/list_devices.c', 'device.c', dependencies: [gtkdep])
+executable('test_camera', 'tools/test_camera.c', 'camera.c', 'device.c', dependencies: [gtkdep])

pipeline.c

@@ -0,0 +1,137 @@
+#include "pipeline.h"
+
+#include <gtk/gtk.h>
+#include <glib-unix.h>
+#include <assert.h>
+
+struct _MPPipeline {
+    GMainContext *main_context;
+    GMainLoop *main_loop;
+    pthread_t thread;
+};
+
+static void *thread_main_loop(void *arg)
+{
+    MPPipeline *pipeline = arg;
+
+    g_main_loop_run(pipeline->main_loop);
+    return NULL;
+}
+
+MPPipeline *mp_pipeline_new()
+{
+    MPPipeline *pipeline = malloc(sizeof(MPPipeline));
+    pipeline->main_context = g_main_context_new();
+    pipeline->main_loop = g_main_loop_new(pipeline->main_context, false);
+    int res = pthread_create(
+        &pipeline->thread, NULL, thread_main_loop, pipeline);
+    assert(res == 0);
+
+    return pipeline;
+}
+
+struct invoke_args {
+    MPPipeline *pipeline;
+    void (*callback)(MPPipeline *, void *);
+};
+
+static bool invoke_impl(struct invoke_args *args)
+{
+    args->callback(args->pipeline, args + 1);
+    return false;
+}
+
+void mp_pipeline_invoke(MPPipeline *pipeline, MPPipelineCallback callback, void *data, size_t size)
+{
+    if (pthread_self() != pipeline->thread) {
+        struct invoke_args *args = malloc(sizeof(struct invoke_args) + size);
+        args->pipeline = pipeline;
+        args->callback = callback;
+
+        if (size > 0) {
+            memcpy(args + 1, data, size);
+        }
+
+        g_main_context_invoke_full(
+            pipeline->main_context,
+            G_PRIORITY_DEFAULT,
+            (GSourceFunc)invoke_impl,
+            args,
+            free);
+    } else {
+        callback(pipeline, data);
+    }
+}
+
+void mp_pipeline_free(MPPipeline *pipeline)
+{
+    g_main_loop_quit(pipeline->main_loop);
+
+    // Force the main thread loop to wake up, otherwise we might not exit
+    g_main_context_wakeup(pipeline->main_context);
+
+    void *r;
+    pthread_join(pipeline->thread, &r);
+    free(pipeline);
+}
+
+struct _MPPipelineCapture {
+    MPPipeline *pipeline;
+    MPCamera *camera;
+
+    void (*callback)(MPImage, void *);
+    void *user_data;
+    GSource *video_source;
+};
+
+static bool on_capture(int fd, GIOCondition condition, MPPipelineCapture *capture)
+{
+    mp_camera_capture_image(capture->camera, capture->callback, capture->user_data);
+    return true;
+}
+
+static void capture_start_impl(MPPipeline *pipeline, MPPipelineCapture **_capture)
+{
+    MPPipelineCapture *capture = *_capture;
+
+    mp_camera_start_capture(capture->camera);
+
+    // Start watching for new captures
+    int video_fd = mp_camera_get_video_fd(capture->camera);
+    capture->video_source = g_unix_fd_source_new(video_fd, G_IO_IN);
+    g_source_set_callback(
+        capture->video_source,
+        (GSourceFunc)on_capture,
+        capture,
+        NULL);
+    g_source_attach(capture->video_source, capture->pipeline->main_context);
+}
+
+MPPipelineCapture *mp_pipeline_capture_start(MPPipeline *pipeline, MPCamera *camera, void (*callback)(MPImage, void *), void *user_data)
+{
+    MPPipelineCapture *capture = malloc(sizeof(MPPipelineCapture));
+    capture->pipeline = pipeline;
+    capture->camera = camera;
+    capture->callback = callback;
+    capture->user_data = user_data;
+    capture->video_source = NULL;
+
+    mp_pipeline_invoke(pipeline, (MPPipelineCallback)capture_start_impl, &capture, sizeof(MPPipelineCapture *));
+
+    return capture;
+}
+
+static void capture_end_impl(MPPipeline *pipeline, MPPipelineCapture **_capture)
+{
+    MPPipelineCapture *capture = *_capture;
+
+    mp_camera_stop_capture(capture->camera);
+    g_source_destroy(capture->video_source);
+
+    free(capture);
+}
+
+void mp_pipeline_capture_end(MPPipelineCapture *capture)
+{
+    mp_pipeline_invoke(capture->pipeline, (MPPipelineCallback)capture_end_impl, &capture, sizeof(MPPipelineCapture *));
+}

pipeline.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "camera.h"
+#include "device.h"
+
+typedef struct _MPPipeline MPPipeline;
+
+typedef void (*MPPipelineCallback)(MPPipeline *, void *);
+
+MPPipeline *mp_pipeline_new();
+void mp_pipeline_invoke(MPPipeline *pipeline, MPPipelineCallback callback, void *data, size_t size);
+void mp_pipeline_free(MPPipeline *pipeline);
+
+typedef struct _MPPipelineCapture MPPipelineCapture;
+
+MPPipelineCapture *mp_pipeline_capture_start(MPPipeline *pipeline, MPCamera *camera, void (*capture)(MPImage, void *), void *data);
+void mp_pipeline_capture_end(MPPipelineCapture *capture);

tools/list_devices.c

@@ -0,0 +1,51 @@
+#include "device.h"
+#include <linux/media.h>
+#include <stdio.h>
+
+int main(int argc, char *argv[]) {
+    MPDeviceList *list = mp_device_list_new();
+
+    while (list) {
+        MPDevice *device = mp_device_list_get(list);
+
+        const struct media_device_info *info = mp_device_get_info(device);
+        printf("%s (%s) %s\n", info->model, info->driver, info->serial);
+        printf("  Bus Info: %s\n", info->bus_info);
+        printf("  Media Version: %d\n", info->media_version);
+        printf("  HW Revision: %d\n", info->hw_revision);
+        printf("  Driver Version: %d\n", info->driver_version);
+
+
+        const struct media_v2_entity *entities = mp_device_get_entities(device);
+        size_t num = mp_device_get_num_entities(device);
+        printf("  Entities (%ld):\n", num);
+        for (int i = 0; i < num; ++i) {
+            printf("    %d %s (%d)\n", entities[i].id, entities[i].name, entities[i].function);
+        }
+
+        const struct media_v2_interface *interfaces = mp_device_get_interfaces(device);
+        num = mp_device_get_num_interfaces(device);
+        printf("  Interfaces (%ld):\n", num);
+        for (int i = 0; i < num; ++i) {
+            printf("    %d (%d - %d) devnode %d:%d\n", interfaces[i].id, interfaces[i].intf_type, interfaces[i].flags, interfaces[i].devnode.major, interfaces[i].devnode.minor);
+        }
+
+        const struct media_v2_pad *pads = mp_device_get_pads(device);
+        num = mp_device_get_num_pads(device);
+        printf("  Pads (%ld):\n", num);
+        for (int i = 0; i < num; ++i) {
+            printf("    %d for device:%d (%d)\n", pads[i].id, pads[i].entity_id, pads[i].flags);
+        }
+
+        const struct media_v2_link *links = mp_device_get_links(device);
+        num = mp_device_get_num_links(device);
+        printf("  Links (%ld):\n", num);
+        for (int i = 0; i < num; ++i) {
+            printf("    %d from:%d to:%d (%d)\n", links[i].id, links[i].source_id, links[i].sink_id, links[i].flags);
+        }
+
+        list = mp_device_list_next(list);
+    }
+
+    mp_device_list_free(list);
+}

tools/test_camera.c

@@ -0,0 +1,171 @@
+#include "camera.h"
+#include "device.h"
+#include <fcntl.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+double get_time()
+{
+    struct timeval t;
+    struct timezone tzp;
+    gettimeofday(&t, &tzp);
+    return t.tv_sec + t.tv_usec*1e-6;
+}
+
+void on_capture(MPImage image, void *user_data)
+{
+    size_t num_bytes = mp_pixel_format_bytes_per_pixel(image.pixel_format) * image.width * image.height;
+    uint8_t *data = malloc(num_bytes);
+    memcpy(data, image.data, num_bytes);
+
+    printf("      first byte: %d.", data[0]);
+
+    free(data);
+}
+
+int main(int argc, char *argv[])
+{
+    if (argc != 2 && argc != 3) {
+        printf("Usage: ./test_camera <media_device_name> [<sub_device_name>]\n");
+        return 1;
+    }
+
+    char *video_name = argv[1];
+    char *subdev_name = NULL;
+    if (argc == 3) {
+        subdev_name = argv[2];
+    }
+
+    double find_start = get_time();
+
+    // First find the device
+    MPDevice *device = mp_device_find(video_name);
+    if (!device) {
+        printf("Device not found\n");
+        return 1;
+    }
+
+    double find_end = get_time();
+
+    printf("Finding the device took %fms\n", (find_end - find_start) * 1000);
+
+    int video_fd;
+    uint32_t video_entity_id;
+    {
+        const struct media_v2_entity *entity = mp_device_find_entity(device, video_name);
+        if (!entity) {
+            printf("Unable to find video device interface\n");
+            return 1;
+        }
+
+        video_entity_id = entity->id;
+
+        const struct media_v2_interface *iface = mp_device_find_entity_interface(device, video_entity_id);
+
+        char buf[256];
+        if (!mp_find_device_path(iface->devnode, buf, 256)) {
+            printf("Unable to find video device path\n");
+            return 1;
+        }
+
+        video_fd = open(buf, O_RDWR);
+        if (video_fd == -1) {
+            printf("Unable to open video device\n");
+            return 1;
+        }
+    }
+
+    int subdev_fd = -1;
+    if (subdev_name)
+    {
+        const struct media_v2_entity *entity = mp_device_find_entity(device, subdev_name);
+        if (!entity) {
+            printf("Unable to find sub-device\n");
+            return 1;
+        }
+
+        const struct media_v2_pad *source_pad = mp_device_get_pad_from_entity(device, entity->id);
+        const struct media_v2_pad *sink_pad = mp_device_get_pad_from_entity(device, video_entity_id);
+
+        // Disable other links
+        const struct media_v2_entity *entities = mp_device_get_entities(device);
+        for (int i = 0; i < mp_device_get_num_entities(device); ++i) {
+            if (entities[i].id != video_entity_id && entities[i].id != entity->id) {
+                const struct media_v2_pad *pad = mp_device_get_pad_from_entity(device, entities[i].id);
+                mp_device_setup_link(device, pad->id, sink_pad->id, false);
+            }
+        }
+
+        // Then enable ours
+        mp_device_setup_link(device, source_pad->id, sink_pad->id, true);
+
+        const struct media_v2_interface *iface = mp_device_find_entity_interface(device, entity->id);
+
+        char buf[256];
+        if (!mp_find_device_path(iface->devnode, buf, 256)) {
+            printf("Unable to find sub-device path\n");
+            return 1;
+        }
+
+        subdev_fd = open(buf, O_RDWR);
+        if (subdev_fd == -1) {
+            printf("Unable to open sub-device\n");
+            return 1;
+        }
+    }
+
+    double open_end = get_time();
+
+    printf("Opening the device took %fms\n", (open_end - find_end) * 1000);
+
+    MPCamera *camera = mp_camera_new(video_fd, subdev_fd);
+
+    MPCameraModeList *modes = mp_camera_list_available_modes(camera);
+
+    double list_end = get_time();
+
+    printf("Available modes: (took %fms)\n", (list_end - open_end) * 1000);
+    for (MPCameraModeList *mode = modes; mode; mode = mp_camera_mode_list_next(mode)) {
+        MPCameraMode *m = mp_camera_mode_list_get(mode);
+        printf("  %dx%d interval:%d/%d fmt:%s\n", m->width, m->height, m->frame_interval.numerator, m->frame_interval.denominator, mp_pixel_format_to_str(m->pixel_format));
+
+        if (m->frame_interval.denominator < 15 || m->frame_interval.denominator > 30) {
+            printf("    Skipping…\n");
+            continue;
+        }
+
+        double start_capture = get_time();
+
+        mp_camera_set_mode(camera, m);
+        mp_camera_start_capture(camera);
+
+        double last = get_time();
+        printf("    Testing 10 captures, starting took %fms\n", (last - start_capture) * 1000);
+
+        for (int i = 0; i < 10; ++i) {
+            mp_camera_capture_image(camera, on_capture, NULL);
+
+            double now = get_time();
+            printf(" capture took %fms\n", (now - last) * 1000);
+            last = now;
+        }
+
+        mp_camera_stop_capture(camera);
+    }
+
+    double cleanup_start = get_time();
+
+    mp_camera_free(camera);
+
+    close(video_fd);
+    if (subdev_fd != -1)
+        close(subdev_fd);
+
+    mp_device_close(device);
+
+    double cleanup_end = get_time();
+
+    printf("Cleanup took %fms\n", (cleanup_end - cleanup_start) * 1000);
+}