diff --git a/camera.c b/camera.c new file mode 100644 index 0000000..20362b9 --- /dev/null +++ b/camera.c @@ -0,0 +1,665 @@ +#include "camera.h" + +#include +#include +#include +#include +#include + +#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); + } +} diff --git a/camera.h b/camera.h new file mode 100644 index 0000000..7be616c --- /dev/null +++ b/camera.h @@ -0,0 +1,68 @@ +#pragma once + +#include +#include +#include + +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); diff --git a/device.c b/device.c new file mode 100644 index 0000000..99634d7 --- /dev/null +++ b/device.c @@ -0,0 +1,402 @@ +#include "device.h" + +#include +#include +#include +#include +#include +#include +#include + +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; +} diff --git a/device.h b/device.h new file mode 100644 index 0000000..0276da8 --- /dev/null +++ b/device.h @@ -0,0 +1,48 @@ +#pragma once + +#include +#include +#include +#include + +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); diff --git a/meson.build b/meson.build index 3cc25ca..f3cdb14 100644 --- a/meson.build +++ b/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]) diff --git a/pipeline.c b/pipeline.c new file mode 100644 index 0000000..3acc6e3 --- /dev/null +++ b/pipeline.c @@ -0,0 +1,137 @@ +#include "pipeline.h" + +#include +#include +#include + +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 *)); +} diff --git a/pipeline.h b/pipeline.h new file mode 100644 index 0000000..a552b26 --- /dev/null +++ b/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); diff --git a/tools/list_devices.c b/tools/list_devices.c new file mode 100644 index 0000000..750531e --- /dev/null +++ b/tools/list_devices.c @@ -0,0 +1,51 @@ +#include "device.h" +#include +#include + +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); +} diff --git a/tools/test_camera.c b/tools/test_camera.c new file mode 100644 index 0000000..22d5414 --- /dev/null +++ b/tools/test_camera.c @@ -0,0 +1,171 @@ +#include "camera.h" +#include "device.h" +#include +#include +#include +#include +#include + +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 []\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); +}