263 lines
7.0 KiB
C
263 lines
7.0 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 "libmegapixels.h"
|
|
#include "log.h"
|
|
#include "util.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) {
|
|
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, sink_entity;
|
|
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)
|
|
{
|
|
for (int i = 0; i < mode->num_cmds; i++) {
|
|
libmegapixels_cmd *cmd = mode->cmds[i];
|
|
struct v4l2_subdev_format subdev_fmt = {};
|
|
switch (cmd->type) {
|
|
case LIBMEGAPIXELS_CMD_LINK:
|
|
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:
|
|
subdev_fmt.pad = cmd->pad_to;
|
|
subdev_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
|
|
subdev_fmt.format.width = cmd->width;
|
|
subdev_fmt.format.height = cmd->height;
|
|
subdev_fmt.format.code = mode->media_busfmt;
|
|
subdev_fmt.format.field = V4L2_FIELD_ANY;
|
|
|
|
libmegapixels_subdev *sd;
|
|
int 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_FMT, &subdev_fmt) == -1) {
|
|
log_error("Could not set mode on sensor: %s\n", strerror(errno));
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
struct v4l2_format format = {0};
|
|
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 bridge: %s\n", strerror(errno));
|
|
return 0;
|
|
}
|
|
return format.fmt.pix.sizeimage;
|
|
} |