Megapixels/io_pipeline.c

#include "io_pipeline.h"

#include "device.h"
#include "camera.h"
#include "pipeline.h"
#include "process_pipeline.h"
#include <string.h>
#include <glib.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <errno.h>
#include <assert.h>
#include <stdio.h>

struct media_link_info {
    unsigned int source_entity_id;
    unsigned int target_entity_id;
    char source_fname[260];
    char target_fname[260];
};

struct camera_info {
    size_t device_index;

    unsigned int pad_id;

    char dev_fname[260];
    int fd;

    MPCamera *camera;

    int gain_ctrl;
    int gain_max;

    bool has_auto_focus_continuous;
    bool has_auto_focus_start;


    // unsigned int entity_id;
    // enum v4l2_buf_type type;

    // char media_dev_fname[260];
    // char video_dev_fname[260];
    // int media_fd;


    // struct mp_media_link media_links[MP_MAX_LINKS];
    // int num_media_links;

    // int gain_ctrl;
};

struct device_info {
    const char *media_dev_name; // owned by camera config

    MPDevice *device;

    unsigned int interface_pad_id;

    int video_fd;
};

static struct camera_info cameras[MP_MAX_CAMERAS];

static struct device_info devices[MP_MAX_CAMERAS];
static size_t num_devices = 0;

static const struct mp_camera_config *camera = NULL;
static MPCameraMode mode;

static bool just_switched_mode = false;
static int blank_frame_count = 0;

static int burst_length;
static int captures_remaining = 0;

static int preview_width;
static int preview_height;

struct control_state {
    bool gain_is_manual;
    int gain;

    bool exposure_is_manual;
    int exposure;
};

static struct control_state desired_controls = {};
static struct control_state current_controls = {};

static bool want_focus = false;

static MPPipeline *pipeline;
static GSource *capture_source;

static int
xioctl(int fd, int request, void *arg)
{
    int r;
    do {
        r = ioctl(fd, request, arg);
    } while (r == -1 && errno == EINTR);
    return r;
}

static int
v4l2_ctrl_set(int fd, uint32_t id, int val)
{
   struct v4l2_control ctrl = {0};
   ctrl.id = id;
   ctrl.value = val;

   if (xioctl(fd, VIDIOC_S_CTRL, &ctrl) == -1) {
       g_printerr("Failed to set control %d to %d\n", id, val);
       return -1;
   }
   return 0;
}

static int
v4l2_ctrl_get(int fd, uint32_t id)
{
   struct v4l2_control ctrl = {0};
   ctrl.id = id;

   if (xioctl(fd, VIDIOC_G_CTRL, &ctrl) == -1) {
       g_printerr("Failed to get control %d\n", id);
       return -1;
   }
   return ctrl.value;
}

static int
v4l2_ctrl_get_max(int fd, uint32_t id)
{
   struct v4l2_queryctrl queryctrl;
   int ret;

   memset(&queryctrl, 0, sizeof(queryctrl));

   queryctrl.id = id;
   ret = xioctl(fd, VIDIOC_QUERYCTRL, &queryctrl);
   if (ret)
       return 0;

   if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
       return 0;
   }

   return queryctrl.maximum;
}

static int
v4l2_has_control(int fd, int control_id)
{
   struct v4l2_queryctrl queryctrl;
   int ret;

   memset(&queryctrl, 0, sizeof(queryctrl));

   queryctrl.id = control_id;
   ret = xioctl(fd, VIDIOC_QUERYCTRL, &queryctrl);
   if (ret)
       return 0;

   if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
       return 0;
   }

   return 1;
}

static void setup_camera(MPDeviceList **device_list, const struct mp_camera_config *config)
{
    // Find device info
    size_t device_index = 0;
    for (; device_index < num_devices; ++device_index) {
        if (strcmp(config->media_dev_name, devices[device_index].media_dev_name) == 0) {
            break;
        }
    }

    if (device_index == num_devices)
    {
        device_index = num_devices;

        // Initialize new device
        struct device_info *info = &devices[device_index];
        info->media_dev_name = config->media_dev_name;
        info->device = mp_device_list_find_remove(device_list, info->media_dev_name);
        if (!info->device) {
            g_printerr("Could not find /dev/media* node matching '%s'\n", info->media_dev_name);
            exit(EXIT_FAILURE);
        }

        const struct media_v2_entity *entity = mp_device_find_entity(info->device, info->media_dev_name);
        if (!entity) {
            g_printerr("Count not find device video entity\n");
            exit(EXIT_FAILURE);
        }

        const struct media_v2_pad *pad = mp_device_get_pad_from_entity(info->device, entity->id);
        info->interface_pad_id = pad->id;

        const struct media_v2_interface *interface = mp_device_find_entity_interface(info->device, entity->id);
        char dev_name[260];
        if (!mp_find_device_path(interface->devnode, dev_name, 260)) {
            g_printerr("Count not find video path\n");
            exit(EXIT_FAILURE);
        }

        info->video_fd = open(dev_name, O_RDWR);
        if (info->video_fd == -1) {
            g_printerr("Could not open %s\n", dev_name);
            exit(EXIT_FAILURE);
        }

        ++num_devices;
    }

    {
        struct camera_info *info = &cameras[config->index];
        struct device_info *dev_info = &devices[device_index];

        info->device_index = device_index;

        const struct media_v2_entity *entity = mp_device_find_entity(dev_info->device, config->dev_name);
        if (!entity) {
            g_printerr("Count not find camera entity matching '%s'\n", config->dev_name);
            exit(EXIT_FAILURE);
        }

        const struct media_v2_pad *pad = mp_device_get_pad_from_entity(dev_info->device, entity->id);

        info->pad_id = pad->id;

        // Make sure the camera starts out as disabled
        mp_device_setup_link(
            dev_info->device,
            info->pad_id,
            dev_info->interface_pad_id,
            false);

        const struct media_v2_interface *interface = mp_device_find_entity_interface(dev_info->device, entity->id);

        if (!mp_find_device_path(interface->devnode, info->dev_fname, 260)) {
            g_printerr("Count not find camera device path\n");
            exit(EXIT_FAILURE);
        }

        info->fd = open(info->dev_fname, O_RDWR);
        if (info->fd == -1) {
            g_printerr("Could not open %s\n", info->dev_fname);
            exit(EXIT_FAILURE);
        }

        info->camera = mp_camera_new(dev_info->video_fd, info->fd);

        // Trigger continuous auto focus if the sensor supports it
        if (v4l2_has_control(info->fd, V4L2_CID_FOCUS_AUTO)) {
            info->has_auto_focus_continuous = true;
            v4l2_ctrl_set(info->fd, V4L2_CID_FOCUS_AUTO, 1);
        }
        if (v4l2_has_control(info->fd, V4L2_CID_AUTO_FOCUS_START)) {
            info->has_auto_focus_start = true;
        }

        if (v4l2_has_control(info->fd, V4L2_CID_GAIN)) {
            info->gain_ctrl = V4L2_CID_GAIN;
            info->gain_max = v4l2_ctrl_get_max(info->fd, V4L2_CID_GAIN);
        }

        if (v4l2_has_control(info->fd, V4L2_CID_ANALOGUE_GAIN)) {
            info->gain_ctrl = V4L2_CID_ANALOGUE_GAIN;
            info->gain_max = v4l2_ctrl_get_max(info->fd, V4L2_CID_ANALOGUE_GAIN);
        }
    }
}

static void setup(MPPipeline *pipeline, const void *data)
{
    MPDeviceList *device_list = mp_device_list_new();

    for (size_t i = 0; i < MP_MAX_CAMERAS; ++i) {
        const struct mp_camera_config *config = mp_get_camera_config(i);
        if (!config) {
            break;
        }

        setup_camera(&device_list, config);
    }

    mp_device_list_free(device_list);
}

void mp_io_pipeline_start()
{
    mp_process_pipeline_start();

    pipeline = mp_pipeline_new();

    mp_pipeline_invoke(pipeline, setup, NULL, 0);
}

void mp_io_pipeline_stop()
{
    if (capture_source) {
        g_source_destroy(capture_source);
    }

    mp_pipeline_free(pipeline);

    mp_process_pipeline_stop();
}

static void
update_process_pipeline()
{
    struct camera_info *info = &cameras[camera->index];

    // Grab the latest control values
    if (!current_controls.gain_is_manual) {
        current_controls.gain = v4l2_ctrl_get(info->fd, info->gain_ctrl);
    }
    if (!current_controls.exposure_is_manual) {
        current_controls.exposure = v4l2_ctrl_get(info->fd, V4L2_CID_EXPOSURE);
    }

    struct mp_process_pipeline_state pipeline_state = {
        .camera = camera,
        .mode = mode,
        .burst_length = burst_length,
        .preview_width = preview_width,
        .preview_height = preview_height,
        .gain_is_manual = current_controls.gain_is_manual,
        .gain = current_controls.gain,
        .gain_max = info->gain_max,
        .exposure_is_manual = current_controls.exposure_is_manual,
        .exposure = current_controls.exposure,
        .has_auto_focus_continuous = info->has_auto_focus_continuous,
        .has_auto_focus_start = info->has_auto_focus_start,
    };
    mp_process_pipeline_update_state(&pipeline_state);
}

static void
focus(MPPipeline *pipeline, const void *data)
{
    want_focus = true;
}

void mp_io_pipeline_focus()
{
    mp_pipeline_invoke(pipeline, focus, NULL, 0);
}

static void
capture(MPPipeline *pipeline, const void *data)
{
    struct camera_info *info = &cameras[camera->index];

    captures_remaining = burst_length;

    // Disable the autogain/exposure while taking the burst
    v4l2_ctrl_set(info->fd, V4L2_CID_AUTOGAIN, 0);
    v4l2_ctrl_set(info->fd, V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL);

    // Change camera mode for capturing
    mp_camera_stop_capture(info->camera);

    mode = camera->capture_mode;
    mp_camera_set_mode(info->camera, &mode);
    just_switched_mode = true;

    mp_camera_start_capture(info->camera);

    update_process_pipeline();

    mp_process_pipeline_capture();
}

void mp_io_pipeline_capture()
{
    mp_pipeline_invoke(pipeline, capture, NULL, 0);
}

static void
update_controls()
{
    // Don't update controls while capturing
    if (captures_remaining > 0) {
        return;
    }

    struct camera_info *info = &cameras[camera->index];

    if (want_focus) {
        if (info->has_auto_focus_continuous) {
            v4l2_ctrl_set(info->fd, V4L2_CID_FOCUS_AUTO, 1);
        } else if (info->has_auto_focus_start) {
            v4l2_ctrl_set(info->fd, V4L2_CID_AUTO_FOCUS_START, 1);
        }

        want_focus = false;
    }

    if (current_controls.gain_is_manual != desired_controls.gain_is_manual) {
        v4l2_ctrl_set(info->fd, V4L2_CID_AUTOGAIN, desired_controls.gain_is_manual ? 0 : 1);
    }

    if (!desired_controls.gain_is_manual && current_controls.gain != desired_controls.gain) {
        v4l2_ctrl_set(info->fd, info->gain_ctrl, desired_controls.gain);
    }

    if (current_controls.exposure_is_manual != desired_controls.exposure_is_manual) {
        v4l2_ctrl_set(
            info->fd,
            V4L2_CID_EXPOSURE_AUTO,
            desired_controls.exposure_is_manual ? V4L2_EXPOSURE_MANUAL : V4L2_EXPOSURE_AUTO);
    }

    if (!desired_controls.exposure_is_manual && current_controls.exposure != desired_controls.exposure) {
        printf("Setting exposure to %d\n", desired_controls.exposure);
        v4l2_ctrl_set(info->fd, V4L2_CID_EXPOSURE, desired_controls.exposure);
    }

    current_controls = desired_controls;
}

static void
on_frame(MPImage image, void *data)
{
    // Only update controls right after a frame was captured
    update_controls();

    // When the mode is switched while capturing we get a couple blank frames,
    // presumably from buffers made ready during the switch. Ignore these.
    if (just_switched_mode)
    {
        if (blank_frame_count < 20) {
            // Only check a 50x50 area
            size_t test_size = MIN(50, image.width) * MIN(50, image.height);

            bool image_is_blank = true;
            for (size_t i = 0; i < test_size; ++i) {
                if (image.data[i] != 0) {
                    image_is_blank = false;
                }
            }

            if (image_is_blank) {
                printf("Skipping blank image\n");
                ++blank_frame_count;
                return;
            }
        } else {
            printf("Blank image limit reached, resulting capture may be blank\n");
        }

        just_switched_mode = false;
        blank_frame_count = 0;
    }

    // Copy from the camera buffer
    size_t size = mp_pixel_format_width_to_bytes(image.pixel_format, image.width) * image.height;
    uint8_t *buffer = malloc(size);
    memcpy(buffer, image.data, size);

    image.data = buffer;

    // Send the image off for processing
    mp_process_pipeline_process_image(image);

    if (captures_remaining > 0) {
        --captures_remaining;

        if (captures_remaining == 0) {
            struct camera_info *info = &cameras[camera->index];

            // Restore the auto exposure and gain if needed
            if (!current_controls.exposure_is_manual) {
                v4l2_ctrl_set(info->fd, V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_AUTO);
            }

            if (!current_controls.gain_is_manual) {
                v4l2_ctrl_set(info->fd, V4L2_CID_AUTOGAIN, 1);
            }

            // Go back to preview mode
            mp_camera_stop_capture(info->camera);

            mode = camera->preview_mode;
            mp_camera_set_mode(info->camera, &mode);
            just_switched_mode = true;

            mp_camera_start_capture(info->camera);

            update_process_pipeline();
        }
    }
}

static void
update_state(MPPipeline *pipeline, const struct mp_io_pipeline_state *state)
{
    // Make sure the state isn't updated more than it needs to be by checking
    // whether this state change actually changes anything.
    bool has_changed = false;

    if (camera != state->camera) {
        has_changed = true;

        if (camera) {
            struct camera_info *info = &cameras[camera->index];
            struct device_info *dev_info = &devices[info->device_index];

            mp_camera_stop_capture(info->camera);
            mp_device_setup_link(
                dev_info->device,
                info->pad_id,
                dev_info->interface_pad_id,
                false);
        }

        if (capture_source) {
            g_source_destroy(capture_source);
            capture_source = NULL;
        }

        camera = state->camera;

        if (camera) {
            struct camera_info *info = &cameras[camera->index];
            struct device_info *dev_info = &devices[info->device_index];

            mp_device_setup_link(
                dev_info->device,
                info->pad_id,
                dev_info->interface_pad_id,
                true);

            mode = camera->preview_mode;
            mp_camera_set_mode(info->camera, &mode);

            mp_camera_start_capture(info->camera);
            capture_source = mp_pipeline_add_capture_source(pipeline, info->camera, on_frame, NULL);

            current_controls.gain_is_manual = v4l2_ctrl_get(info->fd, V4L2_CID_EXPOSURE_AUTO) == V4L2_EXPOSURE_MANUAL;
            current_controls.gain = v4l2_ctrl_get(info->fd, info->gain_ctrl);

            current_controls.exposure_is_manual = v4l2_ctrl_get(info->fd, V4L2_CID_AUTOGAIN) == 0;
            current_controls.exposure = v4l2_ctrl_get(info->fd, V4L2_CID_EXPOSURE);
        }
    }

    has_changed = has_changed
        || burst_length != state->burst_length
        || preview_width != state->preview_width
        || preview_height != state->preview_height;

    burst_length = state->burst_length;
    preview_width = state->preview_width;
    preview_height = state->preview_height;

    if (camera) {
        struct control_state previous_desired = desired_controls;

        desired_controls.gain_is_manual = state->gain_is_manual;
        desired_controls.gain = state->gain;
        desired_controls.exposure_is_manual = state->exposure_is_manual;
        desired_controls.exposure = state->exposure;

        has_changed = has_changed || memcmp(&previous_desired, &desired_controls, sizeof(struct control_state)) != 0;
    }

    assert(has_changed);

    update_process_pipeline();
}

void mp_io_pipeline_update_state(const struct mp_io_pipeline_state *state)
{
    mp_pipeline_invoke(pipeline, (MPPipelineCallback)update_state, state, sizeof(struct mp_io_pipeline_state));
}