301 lines
6.6 KiB
C
301 lines
6.6 KiB
C
/*
|
|
*
|
|
* SPDX-License-Identifier: GPL-2.0
|
|
*
|
|
* Copyright (C) 2020 Amlogic or its affiliates
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; version 2.
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
|
*
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <errno.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <string.h>
|
|
#include <sys/ioctl.h>
|
|
|
|
#include "local_v4l2-subdev.h"
|
|
#include "camera.h"
|
|
|
|
|
|
int subdev_open(struct sub_dev *sdev)
|
|
{
|
|
int fd = -1;
|
|
|
|
fd = open(sdev->devname, O_RDWR);
|
|
if (fd < 0) {
|
|
printf("Failed to open %s\n", sdev->devname);
|
|
return fd;
|
|
}
|
|
|
|
sdev->fd = fd;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int subdev_enum_mbus_code(struct sub_dev *sdev)
|
|
{
|
|
int i = 0, j = 0;
|
|
int ret = 0;
|
|
struct media_pad *pad = NULL;
|
|
struct v4l2_subdev_mbus_code_enum mbus;
|
|
|
|
for (i = 0; i < ARRAY_SIZE(sdev->entity.pads); i++) {
|
|
pad = &sdev->entity.pads[i];
|
|
if (!pad->used)
|
|
continue;
|
|
|
|
for (j = 0; j < ARRAY_SIZE(pad->fmt); j++) {
|
|
mbus.pad = pad->index;
|
|
mbus.index = j;
|
|
mbus.which = V4L2_SUBDEV_FORMAT_ACTIVE;
|
|
|
|
ret = ioctl(sdev->fd, VIDIOC_SUBDEV_ENUM_MBUS_CODE, &mbus);
|
|
if (ret)
|
|
break;
|
|
|
|
pad->fmt[j].code = mbus.code;
|
|
pad->fmt[j].pad = mbus.pad;
|
|
pad->fmt[j].flags = mbus.flags;
|
|
pad->fmt[j].used = 1;
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
int subdev_enum_frame_size(struct sub_dev *sdev)
|
|
{
|
|
int i, j;
|
|
int ret = 0;
|
|
struct media_pad *pad = NULL;
|
|
struct v4l2_subdev_frame_size_enum fsize;
|
|
|
|
for (i = 0; i < ARRAY_SIZE(sdev->entity.pads); i++) {
|
|
pad = &sdev->entity.pads[i];
|
|
if (!pad->used)
|
|
continue;
|
|
|
|
for (j = 0; j < ARRAY_SIZE(pad->fmt); j++) {
|
|
fsize.index = j;
|
|
fsize.pad = pad->index;
|
|
fsize.code = pad->fmt[j].code;
|
|
fsize.which = V4L2_SUBDEV_FORMAT_ACTIVE;
|
|
|
|
ret = ioctl(sdev->fd, VIDIOC_SUBDEV_ENUM_FRAME_SIZE, &fsize);
|
|
if (ret)
|
|
break;
|
|
|
|
pad->fmt[j].min_width = fsize.min_width;
|
|
pad->fmt[j].min_height = fsize.min_height;
|
|
pad->fmt[j].max_width = fsize.max_width;
|
|
pad->fmt[j].max_height = fsize.max_height;
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int subdev_set_format(struct sub_dev *sdev, uint32_t pad, struct format_info *fmt)
|
|
{
|
|
int ret;
|
|
struct v4l2_subdev_format sfmt = { 0 };
|
|
|
|
sfmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
|
|
sfmt.pad = pad;
|
|
sfmt.format.width = fmt->width;
|
|
sfmt.format.height = fmt->height;
|
|
sfmt.format.code = fmt->code;
|
|
sfmt.format.flags = fmt->flags;
|
|
|
|
ret = ioctl(sdev->fd, VIDIOC_SUBDEV_S_FMT, &sfmt);
|
|
if (ret) {
|
|
printf("Failed to set %s subdev fmt: %s\n", sdev->entity.name, strerror(errno));
|
|
return ret;
|
|
}
|
|
|
|
fmt->width = sfmt.format.width;
|
|
fmt->height = sfmt.format.height;
|
|
fmt->code = sfmt.format.code;
|
|
fmt->flags = sfmt.format.flags;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int subdev_get_format(struct sub_dev *sdev, uint32_t pad, struct format_info *fmt)
|
|
{
|
|
int ret;
|
|
struct v4l2_subdev_format sfmt = { 0 };
|
|
|
|
sfmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
|
|
sfmt.pad = pad;
|
|
|
|
ret = ioctl(sdev->fd, VIDIOC_SUBDEV_G_FMT, &sfmt);
|
|
if (ret) {
|
|
printf("Failed to get %s subdev fmt: %s\n", sdev->entity.name, strerror(errno));
|
|
return ret;
|
|
}
|
|
|
|
fmt->width = sfmt.format.width;
|
|
fmt->height = sfmt.format.height;
|
|
fmt->code = sfmt.format.code;
|
|
fmt->flags = sfmt.format.flags;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int subdev_get_selection(struct sub_dev *sdev, uint32_t pad,
|
|
uint32_t target, struct v4l2_rect *rect)
|
|
{
|
|
int ret;
|
|
struct v4l2_subdev_selection sel = { 0 };
|
|
|
|
sel.which = V4L2_SUBDEV_FORMAT_ACTIVE;
|
|
sel.pad = pad;
|
|
sel.target = target;
|
|
|
|
ret = ioctl(sdev->fd, VIDIOC_SUBDEV_G_SELECTION, &sel);
|
|
if (ret) {
|
|
printf("Failed to get %s subdev selection: %s\n", sdev->entity.name, strerror(errno));
|
|
return ret;
|
|
}
|
|
|
|
*rect = sel.r;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int subdev_set_selection(struct sub_dev *sdev, uint32_t pad,
|
|
uint32_t target, struct v4l2_rect *rect)
|
|
{
|
|
int ret = 0;
|
|
struct v4l2_subdev_selection sel = { 0 };
|
|
|
|
sel.which = V4L2_SUBDEV_FORMAT_ACTIVE;
|
|
sel.pad = pad;
|
|
sel.target = target;
|
|
sel.r = *rect;
|
|
|
|
ret = ioctl(sdev->fd, VIDIOC_SUBDEV_S_SELECTION, &sel);
|
|
if (ret) {
|
|
printf("Failed to set %s subdev selection: %s\n", sdev->entity.name, strerror(errno));
|
|
return ret;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
int sensor_get_format(struct sub_dev *sensor, struct format_info *fmt)
|
|
{
|
|
int i = 0;
|
|
struct media_pad *pad;
|
|
|
|
pad = &sensor->entity.pads[0];
|
|
|
|
for (i = 0; i < ARRAY_SIZE(pad->fmt); i++) {
|
|
if (pad->fmt[i].used == 0)
|
|
continue;
|
|
|
|
fmt->width = pad->fmt[i].max_width;
|
|
fmt->height = pad->fmt[i].max_height;
|
|
fmt->code = pad->fmt[i].code;
|
|
|
|
return 0;
|
|
}
|
|
|
|
return -EINVAL;
|
|
}
|
|
|
|
int subdev_query_ctrls(struct sub_dev *sdev)
|
|
{
|
|
int i = 0;
|
|
int ret = 0;
|
|
struct v4l2_query_ext_ctrl *ctrl = NULL;
|
|
|
|
sdev->qctrls.count = 0;
|
|
|
|
for (i = 0; i < ARRAY_SIZE(sdev->qctrls.qctrls); i++) {
|
|
ctrl = &sdev->qctrls.qctrls[i];
|
|
|
|
ctrl->id |= V4L2_CTRL_FLAG_NEXT_CTRL |
|
|
V4L2_CTRL_FLAG_NEXT_COMPOUND;
|
|
ret = ioctl(sdev->fd, VIDIOC_QUERY_EXT_CTRL, ctrl);
|
|
if (ret)
|
|
break;
|
|
|
|
if (ctrl->type == V4L2_CTRL_TYPE_CTRL_CLASS ||
|
|
ctrl->flags & V4L2_CTRL_FLAG_DISABLED)
|
|
continue;
|
|
|
|
switch (ctrl->type) {
|
|
case V4L2_CTRL_TYPE_INTEGER:
|
|
case V4L2_CTRL_TYPE_BOOLEAN:
|
|
case V4L2_CTRL_TYPE_MENU:
|
|
case V4L2_CTRL_TYPE_BUTTON:
|
|
case V4L2_CTRL_TYPE_INTEGER64:
|
|
case V4L2_CTRL_TYPE_BITMASK:
|
|
case V4L2_CTRL_TYPE_INTEGER_MENU:
|
|
case V4L2_CTRL_TYPE_U8:
|
|
sdev->qctrls.count++;
|
|
break;
|
|
default:
|
|
i--;
|
|
printf("Error to support this ctrl type: 0x%x\n", ctrl->type);
|
|
continue;
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int subdev_set_ctrls(struct sub_dev *sdev, void *ctrls, int count)
|
|
{
|
|
int ret = 0;
|
|
struct v4l2_ext_controls ext_ctrls;
|
|
|
|
memset(&ext_ctrls, 0, sizeof(ext_ctrls));
|
|
|
|
ext_ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
|
|
ext_ctrls.controls = ctrls;
|
|
ext_ctrls.count = count;
|
|
|
|
ret = ioctl(sdev->fd, VIDIOC_S_EXT_CTRLS, &ext_ctrls);
|
|
if (ret)
|
|
printf("%s: Failed to set extern controls: %s\n", sdev->entity.name, strerror(errno));
|
|
|
|
return ret;
|
|
}
|
|
|
|
int subdev_get_ctrls(struct sub_dev *sdev, void *ctrls, int count)
|
|
{
|
|
int ret = 0;
|
|
struct v4l2_ext_controls ext_ctrls;
|
|
|
|
memset(&ext_ctrls, 0, sizeof(ext_ctrls));
|
|
|
|
ext_ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
|
|
ext_ctrls.controls = ctrls;
|
|
ext_ctrls.count = count;
|
|
|
|
ret = ioctl(sdev->fd, VIDIOC_G_EXT_CTRLS, &ext_ctrls);
|
|
if (ret)
|
|
printf("Failed to get extern controls: %s\n", sdev->entity.name);
|
|
|
|
return ret;
|
|
}
|
|
|
|
void subdev_close(struct sub_dev *sdev)
|
|
{
|
|
close(sdev->fd);
|
|
}
|