slapin/pinesrcbin
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Files
a11b16cc835c98165922871c3178d5ed46227986
pinesrcbin/utility.c
Sergey Lapin 7839206c7f Camera selection and mode change fixes
2021-04-01 14:37:45 +03:00

617 lines
18 KiB
C
Raw Blame History

/* insert copyright here *
* THE manual: https://www.kernel.org/doc/html/v5.7/media/uapi/mediactl/media-ioc-g-topology.html
*/
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <linux/media.h>
#include <linux/videodev2.h>
#include <linux/v4l2-subdev.h>
#include <gst/gst.h>
#include "pinesrcbin.h"
#include "utility.h"
static const struct media_v2_link *
gst_pinesrcbin_get_link_from_entity(struct device_data *dev, uint32_t id);
static const struct media_v2_interface *
gst_pinesrcbin_get_interface_by_id(struct device_data *dev, uint32_t id);
static const struct media_v2_interface *
gst_pinesrcbin_get_entity_interface(struct device_data *dev, uint32_t id);
static const struct media_v2_pad *
gst_pinesrcbin_get_pad_from_entity(struct device_data *dev, uint32_t id);
static const struct media_v2_entity *
gst_pinesrcbin_find_media_entity(struct device_data *dev,
const char *driver_name);
static struct device_data *
gst_pinesrcbin_find_media_device(GstPineSrcBin *self, const char *driver_name);
static gboolean
get_device_path(struct media_v2_intf_devnode devnode, char *path, int length);
static gboolean
query_autofocus(int fd);
static void
setup_gain(int fd, int *gain_ctrl, int *gain_max);
static gboolean
camera_control(int fd, uint32_t id, int request, uint32_t *value);
static inline const struct media_v2_pad *
get_device_pad(const struct device_data *dev, uint32_t id);
static gboolean
setup_link(const struct device_data *dev, uint32_t source_pad_id,
uint32_t sink_pad_id, gboolean enabled);
static int alloc_device(struct device_data *dev);
static void disable_all_links(struct device_data *dev);
static void subdev_set_mode(int subdev_fd, int width,
int height,
uint32_t bus_code,
struct v4l2_fract interval);
struct camera_config;
int get_camera_config_no(const char *conf_name);
static struct camera_config *
get_camera_config(const char *conf_name);
struct camera_data;
static struct camera_data *
get_camera_data(const char *conf_name);
static int
xioctl (int fd, int request, void *arg)
{
int r;
do {
r = ioctl (fd, request, arg);
} while (r == -1 && errno == EINTR);
return r;
}
struct camera_config {
char conf_name[128];
char camera_name[128];
int width;
int height;
uint32_t bus_code_video;
uint32_t bus_code_single;
struct v4l2_fract frame_interval;
};
struct camera_data {
struct camera_config *conf;
int subdev_fd;
const struct media_v2_entity *entity;
const struct media_v2_interface *interface;
const struct media_v2_pad *pad;
int gain_ctrl;
int gain_max;
char devnode_path[261];
};
struct camera_config configurations[] = {
{"secondary", "gc2145", 1280, 720, MEDIA_BUS_FMT_UYVY8_2X8, MEDIA_BUS_FMT_SBGGR8_1X8, {1, 15}},
{"primary", "ov5640", 1280, 720, MEDIA_BUS_FMT_UYVY8_2X8, MEDIA_BUS_FMT_SBGGR8_1X8, {1, 30}},
};
struct camera_data cam_store[] = {
{},
{}
};
struct mediabus_to_gst {
uint32_t bus_code;
const char *gst_format;
};
static struct mediabus_to_gst gst_format[] = {
{MEDIA_BUS_FMT_SBGGR8_1X8, "BGGR"},
{MEDIA_BUS_FMT_SGBRG8_1X8, "GBRG"},
{MEDIA_BUS_FMT_SGRBG8_1X8, "GRBG"},
{MEDIA_BUS_FMT_SRGGB8_1X8, "RGGB"},
{MEDIA_BUS_FMT_SBGGR10_1X10, "BGGR10"},
{MEDIA_BUS_FMT_SGBRG10_1X10, "GBRG10"},
{MEDIA_BUS_FMT_SGRBG10_1X10, "GRBG10"},
{MEDIA_BUS_FMT_SRGGB10_1X10, "RGGB10"},
{MEDIA_BUS_FMT_UYVY8_2X8, "UYVY"},
{MEDIA_BUS_FMT_YUYV8_2X8, "YUY2"},
};
void gst_pinesrcbin_get_gst_caps_data(const char * conf_name,
int *width, int * height,
const char **format,
struct v4l2_fract *interval)
{
int i;
const struct camera_config *conf = get_camera_config(conf_name);
*width = conf->width;
*height = conf->height;
*format = NULL;
*interval = conf->frame_interval;
for (i = 0; i < sizeof(gst_format)/sizeof(gst_format[0]); i++) {
if (conf->bus_code_video == gst_format[i].bus_code) {
*format = gst_format[i].gst_format;
return;
}
}
g_assert(*format);
}
static const struct media_v2_link *
gst_pinesrcbin_get_link_from_entity(struct device_data *dev, uint32_t id)
{
int i;
for (i = 0; i < dev->num_links; i++)
if (dev->links[i].sink_id == id)
return &dev->links[i];
return NULL;
}
static const struct media_v2_interface *
gst_pinesrcbin_get_interface_by_id(struct device_data *dev, uint32_t id)
{
int i;
for (i = 0; i < dev->num_interfaces; i++)
if (dev->interfaces[i].id == id)
return &dev->interfaces[i];
return NULL;
}
static const struct media_v2_interface *
gst_pinesrcbin_get_entity_interface(struct device_data *dev, uint32_t id)
{
const struct media_v2_link *link =
gst_pinesrcbin_get_link_from_entity(dev, id);
if (!link)
return NULL;
return gst_pinesrcbin_get_interface_by_id(dev, link->source_id);
}
static const struct media_v2_pad *
gst_pinesrcbin_get_pad_from_entity(struct device_data *dev, uint32_t id)
{
int i;
for (i = 0; i < dev->num_pads; i++)
{
if (dev->pads[i].entity_id == id)
return &dev->pads[i];
}
return NULL;
}
static const struct media_v2_entity *
gst_pinesrcbin_find_media_entity(struct device_data *dev,
const char *driver_name)
{
int length = strlen(driver_name);
int i;
for (i = 0; i < dev->num_entities; i++) {
if (!strncmp(dev->entities[i].name, driver_name, length))
return &dev->entities[i];
}
return NULL;
}
static void disable_all_links(struct device_data *dev)
{
int i, j;
for (i = 0; i < dev->num_entities; i++) {
const struct media_v2_pad *sink_pad =
gst_pinesrcbin_get_pad_from_entity (dev, dev->entities[i].id);
if (!sink_pad)
continue;
if (!(sink_pad->flags & MEDIA_PAD_FL_SINK))
continue;
for (j = 0; j < dev->num_entities; j++) {
if (i == j)
continue;
const struct media_v2_pad *source_pad =
gst_pinesrcbin_get_pad_from_entity (dev, dev->entities[j].id);
if (!source_pad)
continue;
if (!(source_pad->flags & MEDIA_PAD_FL_SOURCE))
continue;
setup_link(dev, source_pad->id, sink_pad->id, FALSE);
}
}
}
static struct device_data *
gst_pinesrcbin_find_media_device(GstPineSrcBin *self, const char *driver_name)
{
int length = strlen(driver_name);
GList *l = self->media_list;
while (l)
{
struct device_data *dev = l->data;
const struct media_device_info *info = &dev->info;
g_print("device %s:%s:%s\n", dev->path, info->driver, driver_name);
if (!strncmp(info->driver, driver_name, length))
return dev;
l = l->next;
}
return NULL;
}
static gboolean
get_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);
fclose(f);
return TRUE;
}
}
fclose(f);
return FALSE;
}
static gboolean
query_autofocus(int fd)
{
struct v4l2_query_ext_ctrl ctrl = {};
ctrl.id = V4L2_CID_FOCUS_AUTO;
int rc = xioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ctrl);
if (rc < 0)
return FALSE;
return TRUE;
}
static void
setup_gain(int fd, int *gain_ctrl, int *gain_max)
{
struct v4l2_query_ext_ctrl ctrl = {};
ctrl.id = V4L2_CID_GAIN;
int rc = xioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ctrl);
if (rc >= 0)
{
*gain_ctrl = ctrl.id;
*gain_max = ctrl.maximum;
return;
}
ctrl.id = V4L2_CID_ANALOGUE_GAIN;
rc = xioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ctrl);
if (rc >= 0)
{
*gain_ctrl = ctrl.id;
*gain_max = ctrl.maximum;
return;
}
camera_control(fd, *gain_ctrl, VIDIOC_S_EXT_CTRLS, gain_max);
}
static gboolean
camera_control(int fd, uint32_t id, int request, uint32_t *value)
{
struct v4l2_ext_control ctrl = {};
ctrl.id = id;
ctrl.value = *value;
struct v4l2_ext_controls ctrls = {
.ctrl_class = 0,
.which = V4L2_CTRL_WHICH_CUR_VAL,
.count = 1,
.controls = &ctrl,
};
if (xioctl(fd, request, &ctrls) == -1)
{
return TRUE;
}
*value = ctrl.value;
return FALSE;
}
static inline const struct media_v2_pad *
get_device_pad(const struct device_data *dev, uint32_t id)
{
int i;
for (i = 0; i < dev->num_pads; i++)
{
if (dev->pads[i].id == id)
{
return &dev->pads[i];
}
}
return NULL;
}
/* TODO - get media device pad and video device pad and set up link */
static gboolean
setup_link(const struct device_data *dev, uint32_t source_pad_id,
uint32_t sink_pad_id, gboolean enabled)
{
const struct media_v2_pad *source_pad = get_device_pad(dev, source_pad_id);
g_return_val_if_fail(source_pad, FALSE);
const struct media_v2_pad *sink_pad = get_device_pad(dev, 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(dev->fd, MEDIA_IOC_SETUP_LINK, &link) == -1)
{
g_printerr("MEDIA_IOC_SETUP_LINK: %d %s", errno, strerror(errno));
return FALSE;
}
return TRUE;
}
static int alloc_device(struct device_data *dev)
{
int rc = -1;
g_print("topology x1\n");
rc = xioctl(dev->fd, MEDIA_IOC_G_TOPOLOGY, &dev->topology);
if (rc < 0)
{
g_error("topology x1 error %d %d %s\n", rc,
errno, strerror(errno));
goto out;
}
dev->entities =
calloc(dev->topology.num_entities,
sizeof(struct media_v2_entity));
dev->num_entities = dev->topology.num_entities;
dev->interfaces =
calloc(dev->topology.num_interfaces,
sizeof(struct media_v2_interface));
dev->num_interfaces = dev->topology.num_interfaces;
dev->pads =
calloc(dev->topology.num_pads, sizeof(struct media_v2_pad));
dev->num_pads = dev->topology.num_pads;
dev->links =
calloc(dev->topology.num_links, sizeof(struct media_v2_link));
dev->num_links = dev->topology.num_links;
dev->topology.ptr_entities = (uint64_t)dev->entities;
dev->topology.ptr_interfaces = (uint64_t)dev->interfaces;
dev->topology.ptr_pads = (uint64_t)dev->pads;
dev->topology.ptr_links = (uint64_t)dev->links;
rc = xioctl(dev->fd, MEDIA_IOC_G_TOPOLOGY, &dev->topology);
if (rc < 0)
goto out;
rc = xioctl(dev->fd, MEDIA_IOC_DEVICE_INFO, &dev->info);
out:
return rc;
}
static void subdev_set_mode(int subdev_fd, int width,
int height,
uint32_t bus_code,
struct v4l2_fract frame_interval)
{
struct v4l2_subdev_frame_interval interval = {};
struct v4l2_subdev_format fmt = {};
gboolean frame_is_set;
interval.pad = 0;
interval.interval = frame_interval;
if (xioctl(subdev_fd,
VIDIOC_SUBDEV_S_FRAME_INTERVAL,
&interval) == -1)
g_error("VIDIOC_SUBDEV_S_FRAME_INTERVAL %d %s\n",
errno, strerror(errno));
frame_is_set =
interval.interval.numerator ==
frame_interval.numerator &&
interval.interval.denominator ==
frame_interval.denominator;
fmt.pad = 0;
fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
fmt.format.width = width;
fmt.format.height = height;
fmt.format.code =
bus_code;
fmt.format.field = V4L2_FIELD_ANY;
if (xioctl(subdev_fd, VIDIOC_SUBDEV_S_FMT, &fmt) == -1)
g_error ("VIDIOC_SUBDEV_S_FMT: %d %s\n", errno, strerror(errno));
if (!frame_is_set) {
if (xioctl(subdev_fd,
VIDIOC_SUBDEV_S_FRAME_INTERVAL,
&interval) == -1)
g_error("VIDIOC_SUBDEV_S_FRAME_INTERVAL %d %s\n",
errno, strerror(errno));
}
}
int
get_camera_config_no(const char *conf_name)
{
int i;
for (i = 0;
i < sizeof(configurations)/sizeof(configurations[0]); i++) {
struct camera_config *d = &configurations[i];
if (!strncmp(d->conf_name, conf_name, sizeof(d->conf_name)))
return i;
}
return -1;
}
static struct camera_config *
get_camera_config(const char *conf_name)
{
int conf = get_camera_config_no(conf_name);
if (conf < 0)
return NULL;
else
return &configurations[conf];
}
static struct camera_data *
get_camera_data(const char *conf_name)
{
int conf = get_camera_config_no(conf_name);
if (conf < 0)
return NULL;
else {
cam_store[conf].conf = &configurations[conf];
return &cam_store[conf];
}
}
void set_camera_control(const char * conf_name, int control, int value)
{
struct camera_data *cam_data = get_camera_data(conf_name);
switch(control) {
case CONTROL_HUE:
g_print("configuring hue = %d\n", value);
camera_control (cam_data->subdev_fd, V4L2_CID_HUE, VIDIOC_S_EXT_CTRLS, &value);
break;
case CONTROL_SATURATION:
g_print("configuring saturation = %d\n", value);
camera_control (cam_data->subdev_fd, V4L2_CID_SATURATION, VIDIOC_S_EXT_CTRLS, &value);
break;
case CONTROL_TEST_PATTERN:
g_print("configuring test pattern = %d\n", value);
camera_control (cam_data->subdev_fd, V4L2_CID_TEST_PATTERN, VIDIOC_S_EXT_CTRLS, &value);
break;
}
}
void gst_pinesrcbin_configure_device(GstPineSrcBin * self, const char *conf_name)
{
struct camera_config *conf;
struct camera_data *cam_data;
self->media_list = NULL;
struct dirent *dir;
conf = get_camera_config(conf_name);
if (!conf)
g_error("invalid camera configuration name %s\n", conf_name);
cam_data = get_camera_data(conf_name);
g_assert(cam_data);
GST_DEBUG_OBJECT (self, "Scanning for media devices");
DIR *d = opendir ("/dev");
while ((dir = readdir (d)) != NULL) {
if (strncmp (dir->d_name, "media", 5) == 0) {
char path[261];
int fd;
snprintf (path, 261, "/dev/%s", dir->d_name);
fd = open (path, O_RDWR);
GST_DEBUG_OBJECT (self, "Opened device: %s: fd=%d\n", path, fd);
if (fd >= 0) {
int rc = -1;
struct device_data *dev = g_new0 (struct device_data, 1);
if (!dev)
goto out;
dev->fd = fd;
strncpy (dev->path, path, sizeof (dev->path));
rc = alloc_device(dev);
if (rc < 0)
goto out;
self->media_list = g_list_append(self->media_list, dev);
GST_DEBUG_OBJECT (self, "added device %s %s\n", dev->path, dev->info.driver);
out:
if (rc < 0 && dev)
g_free (dev);
}
}
}
/* TODO: configure file */
self->media_if = gst_pinesrcbin_find_media_device (self, "sun6i-csi");
if (!self->media_if)
g_error ("can't find device sun6i-csi\n");
/* sink entity */
self->v4l2entity =
gst_pinesrcbin_find_media_entity (self->media_if, "sun6i-csi");
if (!self->v4l2entity)
g_error ("can't find entity sun6i-csi\n");
GST_DEBUG_OBJECT (self, "sink entity: %d %s\n", self->v4l2entity->id, self->v4l2entity->name);
/* This is sink pad (for media controller) */
self->v4l2pad =
gst_pinesrcbin_get_pad_from_entity (self->media_if, self->v4l2entity->id);
GST_DEBUG_OBJECT (self, "sink pad: %d\n", self->v4l2pad->id);
if (!self->v4l2pad)
g_error ("can't find interface pad sun6i-csi\n");
self->v4l2interface =
gst_pinesrcbin_get_entity_interface (self->media_if,
self->v4l2entity->id);
if (!self->v4l2interface)
g_printerr ("can't find interface sun6i-csi\n");
GST_DEBUG_OBJECT (self, "sink interface: %d\n", self->v4l2interface->id);
if (!get_device_path (self->v4l2interface->devnode,
self->devnode_path, sizeof (self->devnode_path)))
g_error ("Failed to get devnode path\n");
GST_DEBUG_OBJECT (self, "sink interface devnode path: %s\n", self->devnode_path);
g_print ("devnode: %s\n", self->devnode_path);
/* /dev/video device fd */
self->fd = open (self->devnode_path, O_RDWR);
if (self->fd < 0)
g_error ("failed to open %s\n", self->devnode_path);
/* TODO: source pad? sink pad? Need to disable active link*/
// setup_link(self->media_if, ,self->v4l2interface->pad_id, false);
disable_all_links(self->media_if);
cam_data->entity = gst_pinesrcbin_find_media_entity(self->media_if, conf->camera_name);
GST_DEBUG_OBJECT (self, "camera: %s: source interface entity %d\n", conf->camera_name, cam_data->entity->id);
cam_data->pad =
gst_pinesrcbin_get_pad_from_entity (self->media_if, cam_data->entity->id);
cam_data->interface =
gst_pinesrcbin_get_entity_interface (self->media_if,
cam_data->entity->id);
GST_DEBUG_OBJECT (self, "camera: %s: source interface %d\n", conf->camera_name, cam_data->interface->id);
if (!get_device_path (cam_data->interface->devnode,
cam_data->devnode_path, sizeof (cam_data->devnode_path)))
g_error ("Failed to get subdevice devnode path for %s\n", conf->camera_name);
GST_DEBUG_OBJECT (self, "source subdevice %s\n", cam_data->devnode_path);
cam_data->subdev_fd = open (cam_data->devnode_path, O_RDWR);
if (cam_data->subdev_fd < 0)
g_error ("Failed to open subdevice: %s\n", cam_data->devnode_path);
self->autofocus = FALSE;
if (query_autofocus (cam_data->subdev_fd)) {
uint32_t enable = 1U;
g_print ("autofocus possible\n");
self->autofocus = TRUE;
camera_control (cam_data->subdev_fd, V4L2_CID_FOCUS_AUTO, VIDIOC_S_EXT_CTRLS, &enable);
}
setup_gain (cam_data->subdev_fd, &cam_data->gain_ctrl, &cam_data->gain_max);
/* self->test_pattern = 1; */
camera_control (cam_data->subdev_fd, V4L2_CID_TEST_PATTERN, VIDIOC_S_EXT_CTRLS, &self->test_pattern);
{
int auto_gain = 1;
camera_control (cam_data->subdev_fd, V4L2_CID_AUTOGAIN, VIDIOC_S_EXT_CTRLS, &auto_gain);
}
set_camera_control(conf_name, CONTROL_HUE, self->hue);
/* now configuring camera modes */
GST_DEBUG_OBJECT (self, "configuring camera modes\n");
subdev_set_mode(cam_data->subdev_fd, conf->width,
conf->height, conf->bus_code_video,
conf->frame_interval);
GST_DEBUG_OBJECT (self, "enabling link for %s\n", conf->camera_name);
setup_link(self->media_if, cam_data->pad->id, self->v4l2pad->id, TRUE);
}
void gst_pinesrcbin_free_device(GstPineSrcBin * self)
{
struct camera_data *cam_data = get_camera_data(self->device);
if (!self->media_if)
return;
close(cam_data->subdev_fd);
close(self->fd);
disable_all_links(self->media_if);
close(self->media_if);
}
Reference in New Issue View Git Blame Copy Permalink