418 lines
13 KiB
C
418 lines
13 KiB
C
#include <fcntl.h>
|
|
#include <errno.h>
|
|
#include <string.h>
|
|
#include <linux/media.h>
|
|
#include <linux/v4l2-subdev.h>
|
|
#include <unistd.h>
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <assert.h>
|
|
#include "libmegapixels.h"
|
|
#include "log.h"
|
|
#include "util.h"
|
|
#include "mode.h"
|
|
|
|
int
|
|
setup_link(libmegapixels_camera *camera, uint32_t source_entity_id, uint32_t sink_entity_id,
|
|
uint16_t source_index, uint16_t sink_index, int enabled)
|
|
{
|
|
struct media_link_desc link = {};
|
|
link.flags = (enabled > 0) ? MEDIA_LNK_FL_ENABLED : 0;
|
|
link.source.entity = source_entity_id;
|
|
link.source.index = source_index;
|
|
link.sink.entity = sink_entity_id;
|
|
link.sink.index = sink_index;
|
|
|
|
if (xioctl(camera->media_fd, MEDIA_IOC_SETUP_LINK, &link) == -1) {
|
|
return -1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
load_entity_ids(libmegapixels_camera *camera)
|
|
{
|
|
// This media device matches on model or driver, scan the entities for the sensor
|
|
struct media_v2_topology topology = {0};
|
|
if (xioctl(camera->media_fd, MEDIA_IOC_G_TOPOLOGY, &topology) == -1 ||
|
|
topology.num_entities == 0) {
|
|
close(camera->media_fd);
|
|
return -1;
|
|
}
|
|
|
|
struct media_v2_entity *entities = calloc(topology.num_entities, sizeof(struct media_v2_entity));
|
|
struct media_v2_interface *interfaces = calloc(topology.num_interfaces, sizeof(struct media_v2_interface));
|
|
struct media_v2_pad *pads = calloc(topology.num_pads, sizeof(struct media_v2_pad));
|
|
struct media_v2_link *links = calloc(topology.num_links, sizeof(struct media_v2_link));
|
|
|
|
topology.ptr_entities = (uint64_t) entities;
|
|
topology.ptr_interfaces = (uint64_t) interfaces;
|
|
topology.ptr_pads = (uint64_t) pads;
|
|
topology.ptr_links = (uint64_t) links;
|
|
|
|
if (xioctl(camera->media_fd, MEDIA_IOC_G_TOPOLOGY, &topology) == -1) {
|
|
close(camera->media_fd);
|
|
return -1;
|
|
}
|
|
|
|
for (int i = 0; i < camera->num_modes; i++) {
|
|
libmegapixels_mode *mode = camera->modes[i];
|
|
|
|
for (int j = 0; j < mode->num_cmds; j++) {
|
|
libmegapixels_cmd *cmd = mode->cmds[j];
|
|
|
|
if (cmd->type == LIBMEGAPIXELS_CMD_LINK) {
|
|
int found_from = 0;
|
|
int found_to = 0;
|
|
for (int k = 0; k < topology.num_entities; k++) {
|
|
if (strncmp(entities[k].name, cmd->entity_from, strlen(cmd->entity_from)) == 0) {
|
|
cmd->entity_from_id = entities[k].id;
|
|
found_from++;
|
|
}
|
|
if (strncmp(entities[k].name, cmd->entity_to, strlen(cmd->entity_to)) == 0) {
|
|
cmd->entity_to_id = entities[k].id;
|
|
found_to++;
|
|
}
|
|
}
|
|
if (found_from != 1) {
|
|
log_error("Could not find entity '%s'\n", cmd->entity_from);
|
|
return -1;
|
|
}
|
|
if (found_to != 1) {
|
|
log_error("Could not find entity '%s'\n", cmd->entity_to);
|
|
return -1;
|
|
}
|
|
} else if (cmd->type == LIBMEGAPIXELS_CMD_MODE || cmd->type == LIBMEGAPIXELS_CMD_CROP ||
|
|
cmd->type == LIBMEGAPIXELS_CMD_INTERVAL) {
|
|
int found = 0;
|
|
for (int k = 0; k < topology.num_entities; k++) {
|
|
if (strncmp(entities[k].name, cmd->entity_from, strlen(cmd->entity_from)) == 0) {
|
|
cmd->entity_from_id = entities[k].id;
|
|
found++;
|
|
break;
|
|
}
|
|
}
|
|
if (found != 1) {
|
|
log_error("Could not find entity '%s'\n", cmd->entity_from);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < topology.num_links; i++) {
|
|
if (links[i].flags & MEDIA_LNK_FL_ENABLED && !(links[i].flags & MEDIA_LNK_FL_IMMUTABLE)) {
|
|
uint32_t source_entity = 0, sink_entity = 0;
|
|
for (int j = 0; j < topology.num_pads; j++) {
|
|
if (pads[j].id == links[i].source_id) {
|
|
source_entity = pads[j].entity_id;
|
|
} else if (pads[j].id == links[i].sink_id) {
|
|
sink_entity = pads[j].entity_id;
|
|
}
|
|
}
|
|
|
|
setup_link(camera, source_entity, sink_entity, 0, 0, 0);
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
libmegapixels_open(libmegapixels_camera *camera)
|
|
{
|
|
if (camera->media_fd != 0) {
|
|
log_error("Camera already opened\n");
|
|
return -1;
|
|
}
|
|
if (camera->sensor_fd != 0) {
|
|
log_error("Sensor already opened\n");
|
|
return -1;
|
|
}
|
|
if (camera->video_fd != 0) {
|
|
log_error("Bridge already opened\n");
|
|
return -1;
|
|
}
|
|
|
|
if (camera->media_path) {
|
|
camera->media_fd = open(camera->media_path, O_RDWR);
|
|
if (camera->media_fd < 0) {
|
|
log_error("Could not open %s: %s\n", camera->media_path, strerror(errno));
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
if (camera->sensor_path) {
|
|
camera->sensor_fd = open(camera->sensor_path, O_RDWR);
|
|
if (camera->sensor_fd < 0) {
|
|
log_error("Could not open %s: %s\n", camera->sensor_path, strerror(errno));
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
camera->video_fd = open(camera->video_path, O_RDWR);
|
|
if (camera->video_fd < 0) {
|
|
log_error("Could not open %s: %s\n", camera->video_path, strerror(errno));
|
|
return -1;
|
|
}
|
|
|
|
// If this is an UVC camera the sensor _is_ the video device
|
|
if (camera->sensor_fd == 0) {
|
|
camera->sensor_fd = camera->video_fd;
|
|
}
|
|
|
|
if (camera->media_fd > 0) {
|
|
int ret = load_entity_ids(camera);
|
|
if (ret < 0) {
|
|
return ret;
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
void
|
|
libmegapixels_close(libmegapixels_camera *camera)
|
|
{
|
|
int uvc = 0;
|
|
if (camera->sensor_fd != 0 && camera->sensor_fd == camera->video_fd) {
|
|
uvc = 1;
|
|
}
|
|
|
|
if (camera->media_fd != 0) {
|
|
close(camera->media_fd);
|
|
camera->media_fd = 0;
|
|
}
|
|
if (camera->sensor_fd != 0) {
|
|
close(camera->sensor_fd);
|
|
camera->sensor_fd = 0;
|
|
if (uvc) {
|
|
camera->video_fd = 0;
|
|
}
|
|
}
|
|
if (camera->video_fd != 0) {
|
|
close(camera->video_fd);
|
|
camera->video_fd = 0;
|
|
}
|
|
|
|
for (int i = 0; i < camera->num_handles; i++) {
|
|
if (camera->handles[i]->fd != 0) {
|
|
close(camera->handles[i]->fd);
|
|
camera->handles[i]->fd = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
unsigned int
|
|
libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode, struct v4l2_format *format)
|
|
{
|
|
assert(camera->video_fd != 0);
|
|
assert(camera->sensor_fd != 0);
|
|
char modename[32] = {0};
|
|
mode_snprintf(modename, 31, mode);
|
|
log_debug("Setting '%s' mode to [%s]\n", camera->name, modename);
|
|
for (int i = 0; i < mode->num_cmds; i++) {
|
|
libmegapixels_cmd *cmd = mode->cmds[i];
|
|
struct v4l2_subdev_format subdev_fmt = {};
|
|
struct v4l2_subdev_crop subdev_crop = {};
|
|
struct v4l2_subdev_frame_interval subdev_ival = {};
|
|
int found = 0;
|
|
libmegapixels_subdev *sd;
|
|
|
|
switch (cmd->type) {
|
|
case LIBMEGAPIXELS_CMD_LINK:
|
|
log_debug(" Link %s:%d -> %s:%d\n", cmd->entity_from, cmd->pad_from, cmd->entity_to, cmd->pad_to);
|
|
if (setup_link(camera, cmd->entity_from_id, cmd->entity_to_id, cmd->pad_from, cmd->pad_to, 1) != 0) {
|
|
log_error("Could not link %d -> %d [%s -> %s] \n", cmd->entity_from_id, cmd->entity_to_id,
|
|
cmd->entity_from,
|
|
cmd->entity_to);
|
|
}
|
|
break;
|
|
case LIBMEGAPIXELS_CMD_MODE:
|
|
log_debug(" Mode %s:%d [%dx%d %s]\n", cmd->entity_from, cmd->pad_from, cmd->width, cmd->height,
|
|
libmegapixels_format_name(cmd->format));
|
|
|
|
found = 0;
|
|
for (int h = 0; h < camera->num_handles; h++) {
|
|
if (camera->handles[h]->entity_id == cmd->entity_from_id) {
|
|
sd = camera->handles[h];
|
|
found++;
|
|
}
|
|
}
|
|
if (found != 1) {
|
|
log_error("Could not find handle for entity\n");
|
|
break;
|
|
}
|
|
|
|
if (sd->fd == 0) {
|
|
sd->fd = open(sd->path, O_RDWR);
|
|
if (sd->fd < 0) {
|
|
log_error("Could not open %s\n", sd->path);
|
|
break;
|
|
}
|
|
}
|
|
|
|
subdev_fmt.pad = cmd->pad_from;
|
|
subdev_fmt.which = V4L2_SUBDEV_FORMAT_TRY;
|
|
subdev_fmt.format.width = cmd->width;
|
|
subdev_fmt.format.height = cmd->height;
|
|
subdev_fmt.format.code = libmegapixels_format_to_media_busfmt(cmd->format);
|
|
subdev_fmt.format.field = V4L2_FIELD_ANY;
|
|
|
|
if (xioctl(sd->fd, VIDIOC_SUBDEV_S_FMT, &subdev_fmt) == -1) {
|
|
log_error("Could not try mode on %s:%d: %s\n", cmd->entity_from, cmd->pad_from, strerror(errno));
|
|
}
|
|
|
|
if (subdev_fmt.format.width != cmd->width || subdev_fmt.format.height != cmd->height) {
|
|
log_error("Driver rejected resolution %dx%d, changed to %dx%d\n", cmd->width, cmd->height,
|
|
subdev_fmt.format.width, subdev_fmt.format.height);
|
|
break;
|
|
}
|
|
if (subdev_fmt.format.code != libmegapixels_format_to_media_busfmt(cmd->format)) {
|
|
log_error("Driver rejected pixfmt try: %d\n", subdev_fmt.format.code);
|
|
break;
|
|
}
|
|
subdev_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
|
|
if (xioctl(sd->fd, VIDIOC_SUBDEV_S_FMT, &subdev_fmt) == -1) {
|
|
log_error("Could not set mode on %s:%d: %s\n", cmd->entity_from, cmd->pad_from, strerror(errno));
|
|
}
|
|
|
|
// There seem to be drivers that don't reject resolutions in V4L2_SUBDEV_FORMAT_TRY mode but
|
|
// do silently change the resolution when doing the V4L2_SUBDEV_FORMAT_ACTIVE bit. So check
|
|
// again since V4L2 is the wild west.
|
|
if (subdev_fmt.format.width != cmd->width || subdev_fmt.format.height != cmd->height) {
|
|
log_error("Driver rejected resolution %dx%d, changed to %dx%d\n", cmd->width, cmd->height,
|
|
subdev_fmt.format.width, subdev_fmt.format.height);
|
|
break;
|
|
}
|
|
if (subdev_fmt.format.code != libmegapixels_format_to_media_busfmt(cmd->format)) {
|
|
log_error("Driver rejected pixfmt: %d\n", subdev_fmt.format.code);
|
|
break;
|
|
}
|
|
break;
|
|
case LIBMEGAPIXELS_CMD_INTERVAL:
|
|
subdev_ival.pad = cmd->pad_from;
|
|
struct v4l2_fract ival;
|
|
ival.numerator = 1;
|
|
ival.denominator = cmd->rate;
|
|
subdev_ival.interval = ival;
|
|
log_debug(" Rate %s:%d [%d]\n", cmd->entity_from, cmd->pad_from, cmd->rate);
|
|
|
|
found = 0;
|
|
for (int h = 0; h < camera->num_handles; h++) {
|
|
if (camera->handles[h]->entity_id == cmd->entity_from_id) {
|
|
sd = camera->handles[h];
|
|
found++;
|
|
}
|
|
}
|
|
if (found != 1) {
|
|
log_error("Could not find handle for entity\n");
|
|
break;
|
|
}
|
|
|
|
if (sd->fd == 0) {
|
|
sd->fd = open(sd->path, O_RDWR);
|
|
if (sd->fd < 0) {
|
|
log_error("Could not open %s\n", sd->path);
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (xioctl(sd->fd, VIDIOC_SUBDEV_S_FRAME_INTERVAL, &subdev_ival) == -1) {
|
|
log_error("Could not set rate on %s:%d: %s\n", cmd->entity_from, cmd->pad_from, strerror(errno));
|
|
}
|
|
break;
|
|
case LIBMEGAPIXELS_CMD_CROP:
|
|
subdev_crop.pad = cmd->pad_from;
|
|
subdev_crop.which = V4L2_SUBDEV_FORMAT_ACTIVE;
|
|
subdev_crop.rect.top = cmd->top;
|
|
subdev_crop.rect.left = cmd->left;
|
|
subdev_crop.rect.width = cmd->width;
|
|
subdev_crop.rect.height = cmd->height;
|
|
|
|
log_debug(" Crop %s:%d [%dx%d+%d+%d]\n", cmd->entity_from, cmd->pad_from, cmd->width, cmd->height,
|
|
cmd->top, cmd->left);
|
|
|
|
found = 0;
|
|
for (int h = 0; h < camera->num_handles; h++) {
|
|
if (camera->handles[h]->entity_id == cmd->entity_from_id) {
|
|
sd = camera->handles[h];
|
|
found++;
|
|
}
|
|
}
|
|
if (found != 1) {
|
|
log_error("Could not find handle for entity\n");
|
|
break;
|
|
}
|
|
|
|
if (sd->fd == 0) {
|
|
sd->fd = open(sd->path, O_RDWR);
|
|
if (sd->fd < 0) {
|
|
log_error("Could not open %s\n", sd->path);
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (xioctl(sd->fd, VIDIOC_SUBDEV_S_CROP, &subdev_crop) == -1) {
|
|
log_error("Could not set crop on entity: %s\n", strerror(errno));
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
struct v4l2_capability cap;
|
|
if (xioctl(camera->video_fd, VIDIOC_QUERYCAP, &cap) == -1) {
|
|
log_error("Could not query %s: %s\n", camera->video_path, strerror(errno));
|
|
return 0;
|
|
}
|
|
if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) {
|
|
format->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
|
|
format->fmt.pix_mp.width = mode->width;
|
|
format->fmt.pix_mp.height = mode->height;
|
|
format->fmt.pix_mp.pixelformat = mode->v4l_pixfmt;
|
|
format->fmt.pix_mp.field = V4L2_FIELD_ANY;
|
|
} else {
|
|
format->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
format->fmt.pix.width = mode->width;
|
|
format->fmt.pix.height = mode->height;
|
|
format->fmt.pix.pixelformat = mode->v4l_pixfmt;
|
|
format->fmt.pix.field = V4L2_FIELD_ANY;
|
|
}
|
|
|
|
if (xioctl(camera->video_fd, VIDIOC_S_FMT, format) == -1) {
|
|
log_error("Could not set mode on %s: %s\n", camera->video_path, strerror(errno));
|
|
return 0;
|
|
}
|
|
if (xioctl(camera->video_fd, VIDIOC_G_FMT, format) == -1) {
|
|
log_error("Could not get mode of %s: %s\n", camera->video_path, strerror(errno));
|
|
return 0;
|
|
}
|
|
if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) {
|
|
if (format->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
|
|
log_error("Capture driver changed capture type\n");
|
|
return 0;
|
|
}
|
|
if (format->fmt.pix_mp.pixelformat != mode->v4l_pixfmt) {
|
|
log_error("Capture driver changed pixfmt to %c%c%c%c\n",
|
|
format->fmt.pix_mp.pixelformat & 0xff,
|
|
format->fmt.pix_mp.pixelformat >> 8 & 0xff,
|
|
format->fmt.pix_mp.pixelformat >> 16 & 0xff,
|
|
format->fmt.pix_mp.pixelformat >> 24 & 0xff);
|
|
return 0;
|
|
}
|
|
} else {
|
|
if (format->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
|
|
log_error("Capture driver changed capture type\n");
|
|
return 0;
|
|
}
|
|
if (format->fmt.pix.pixelformat != mode->v4l_pixfmt) {
|
|
log_error("Capture driver changed pixfmt to %c%c%c%c\n",
|
|
format->fmt.pix.pixelformat & 0xff,
|
|
format->fmt.pix.pixelformat >> 8 & 0xff,
|
|
format->fmt.pix.pixelformat >> 16 & 0xff,
|
|
format->fmt.pix.pixelformat >> 24 & 0xff);
|
|
return 0;
|
|
}
|
|
}
|
|
camera->current_mode = mode;
|
|
|
|
return 1;
|
|
} |