655 lines
15 KiB
C
655 lines
15 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 <stdint.h>
|
|
#include <stdlib.h>
|
|
#include <unistd.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <fcntl.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/stat.h>
|
|
#include <sys/time.h>
|
|
#include <sys/types.h>
|
|
#include <errno.h>
|
|
#include "camera.h"
|
|
#include <sys/sysmacros.h>
|
|
#include "local_media.h"
|
|
|
|
|
|
|
|
#define DEV_RDEV_CACHE_MAX 128
|
|
#define DEV_SCAN_MAX 64
|
|
#define MEDIA_DEVNAME_LEN 32
|
|
|
|
struct dev_rdev_entry {
|
|
unsigned int maj;
|
|
unsigned int min;
|
|
char path[MEDIA_DEVNAME_LEN];
|
|
};
|
|
|
|
static struct dev_rdev_entry dev_rdev_cache[DEV_RDEV_CACHE_MAX];
|
|
static unsigned int dev_rdev_cache_count;
|
|
|
|
static void dev_rdev_cache_clear(void)
|
|
{
|
|
dev_rdev_cache_count = 0;
|
|
}
|
|
|
|
static int dev_rdev_cache_add(unsigned int maj, unsigned int min, const char *path)
|
|
{
|
|
unsigned int i;
|
|
|
|
for (i = 0; i < dev_rdev_cache_count; i++) {
|
|
if (dev_rdev_cache[i].maj == maj && dev_rdev_cache[i].min == min)
|
|
return 0;
|
|
}
|
|
|
|
if (dev_rdev_cache_count >= DEV_RDEV_CACHE_MAX)
|
|
return -ENOMEM;
|
|
|
|
dev_rdev_cache[dev_rdev_cache_count].maj = maj;
|
|
dev_rdev_cache[dev_rdev_cache_count].min = min;
|
|
strncpy(dev_rdev_cache[dev_rdev_cache_count].path, path,
|
|
sizeof(dev_rdev_cache[dev_rdev_cache_count].path) - 1);
|
|
dev_rdev_cache[dev_rdev_cache_count].path[MEDIA_DEVNAME_LEN - 1] = '\0';
|
|
dev_rdev_cache_count++;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static void dev_rdev_probe_prefix(const char *prefix)
|
|
{
|
|
char path[64];
|
|
struct stat st;
|
|
unsigned int i;
|
|
|
|
for (i = 0; i < DEV_SCAN_MAX; i++) {
|
|
snprintf(path, sizeof(path), "/dev/%s%u", prefix, i);
|
|
if (stat(path, &st) != 0)
|
|
continue;
|
|
if (!S_ISCHR(st.st_mode))
|
|
continue;
|
|
dev_rdev_cache_add(major(st.st_rdev), minor(st.st_rdev), path);
|
|
}
|
|
}
|
|
|
|
static void dev_rdev_cache_build(void)
|
|
{
|
|
dev_rdev_cache_clear();
|
|
dev_rdev_probe_prefix("video");
|
|
dev_rdev_probe_prefix("v4l-subdev");
|
|
printf("devpath-v2-no-uevent: rdev cache built, %u entries\n", dev_rdev_cache_count);
|
|
}
|
|
|
|
static int dev_rdev_cache_lookup(unsigned int maj, unsigned int min,
|
|
char *out, size_t out_len)
|
|
{
|
|
unsigned int i;
|
|
|
|
for (i = 0; i < dev_rdev_cache_count; i++) {
|
|
if (dev_rdev_cache[i].maj != maj || dev_rdev_cache[i].min != min)
|
|
continue;
|
|
strncpy(out, dev_rdev_cache[i].path, out_len - 1);
|
|
out[out_len - 1] = '\0';
|
|
return 0;
|
|
}
|
|
|
|
return -ENODEV;
|
|
}
|
|
|
|
static int topo_resolve_devpath_readlink(unsigned int maj, unsigned int min,
|
|
char *devname, size_t len)
|
|
{
|
|
char sysname[64];
|
|
char target[1024];
|
|
char path[MEDIA_DEVNAME_LEN];
|
|
struct stat st;
|
|
const char *p;
|
|
int ret;
|
|
|
|
snprintf(sysname, sizeof(sysname), "/sys/dev/char/%u:%u", maj, min);
|
|
ret = readlink(sysname, target, sizeof(target) - 1);
|
|
if (ret < 0)
|
|
return -errno;
|
|
|
|
target[ret] = '\0';
|
|
p = strrchr(target, '/');
|
|
if (!p)
|
|
return -EINVAL;
|
|
|
|
snprintf(path, sizeof(path), "/dev/%s", p + 1);
|
|
if (stat(path, &st) < 0)
|
|
return -errno;
|
|
|
|
if (major(st.st_rdev) != maj || minor(st.st_rdev) != min)
|
|
return -EINVAL;
|
|
|
|
strncpy(devname, path, len - 1);
|
|
devname[len - 1] = '\0';
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int topo_resolve_devpath(unsigned int maj, unsigned int min,
|
|
char *devname, size_t len)
|
|
{
|
|
int ret;
|
|
|
|
ret = dev_rdev_cache_lookup(maj, min, devname, len);
|
|
if (ret == 0) {
|
|
printf("devpath %u:%u -> %s (cache)\n", maj, min, devname);
|
|
return 0;
|
|
}
|
|
|
|
ret = topo_resolve_devpath_readlink(maj, min, devname, len);
|
|
if (ret == 0) {
|
|
printf("devpath %u:%u -> %s (readlink)\n", maj, min, devname);
|
|
return 0;
|
|
}
|
|
|
|
return -ENODEV;
|
|
}
|
|
|
|
static struct media_v2_interface *topo_find_iface(struct media_v2_topology *topo, uint32_t ett_id)
|
|
{
|
|
int i = 0;
|
|
uint32_t iface_id = 0;
|
|
struct media_v2_link *links = (struct media_v2_link *)topo->ptr_links;
|
|
struct media_v2_interface *ifaces =(struct media_v2_interface *)topo->ptr_interfaces;
|
|
|
|
for (i = 0; i < topo->num_links; i++) {
|
|
if (links[i].sink_id != ett_id)
|
|
continue;
|
|
|
|
if ((links[i].flags & MEDIA_LNK_FL_LINK_TYPE) != MEDIA_LNK_FL_INTERFACE_LINK)
|
|
continue;
|
|
|
|
iface_id = links[i].source_id;
|
|
break;
|
|
}
|
|
|
|
if (i == topo->num_links)
|
|
return NULL;
|
|
|
|
for (i = 0; i < topo->num_interfaces; i++) {
|
|
if (ifaces[i].id == iface_id)
|
|
return &ifaces[i];
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
static int topo_parse_devname(struct media_entity *entity, char *devname, size_t devname_len)
|
|
{
|
|
int ret;
|
|
|
|
if (!entity || !devname || devname_len == 0) {
|
|
printf("Error input param\n");
|
|
return -EINVAL;
|
|
}
|
|
|
|
if (entity->major == 0 && entity->minor == 0) {
|
|
printf("entity %s has no devnode\n", entity->name);
|
|
return -ENODEV;
|
|
}
|
|
|
|
ret = topo_resolve_devpath(entity->major, entity->minor, devname, devname_len);
|
|
if (ret != 0) {
|
|
printf("Failed to resolve dev %u:%u for %s\n", entity->major, entity->minor, entity->name);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
static int topo_parse_entity(struct media_v2_topology *topo, struct topo_param *param)
|
|
{
|
|
int i = 0;
|
|
int ret = 0;
|
|
char *ptr = NULL;
|
|
struct media_v2_interface *iface = NULL;
|
|
struct media_entity *entity = param->entity;
|
|
struct media_v2_entity *entities = (void *)topo->ptr_entities;
|
|
|
|
for (i = 0; i < topo->num_entities; i++) {
|
|
ptr = strstr(entities[i].name, param->ett_name);
|
|
if (ptr == NULL)
|
|
continue;
|
|
|
|
strncpy(entity->name, entities[i].name, sizeof(entity->name));
|
|
entity->id = entities[i].id;
|
|
entity->function = entities[i].function;
|
|
entity->flags = entities[i].flags;
|
|
entity->dev = param->dev;
|
|
break;
|
|
}
|
|
|
|
if (i == topo->num_entities) {
|
|
printf("Error to match entity %s\n", param->ett_name);
|
|
return ret;
|
|
}
|
|
|
|
iface = topo_find_iface(topo, entity->id);
|
|
if (iface) {
|
|
entity->major = iface->devnode.major;
|
|
entity->minor = iface->devnode.minor;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
static int topo_parse_pad(struct media_v2_topology *topo, struct topo_param *param)
|
|
{
|
|
int i, j;
|
|
int ret = 0;
|
|
struct media_entity *entity = param->entity;
|
|
struct media_v2_pad *pads = (void *)topo->ptr_pads;
|
|
|
|
for (i = 0; i < topo->num_pads; i++) {
|
|
if (pads[i].entity_id != entity->id)
|
|
continue;
|
|
|
|
for (j = 0; j < ARRAY_SIZE(entity->pads); j++) {
|
|
if (entity->pads[j].used)
|
|
continue;
|
|
|
|
entity->pads[j].id = pads[i].id;
|
|
entity->pads[j].entity_id = pads[i].entity_id;
|
|
entity->pads[j].flags = pads[i].flags;
|
|
entity->pads[j].index = pads[i].index;
|
|
entity->pads[j].entity = entity;
|
|
entity->pads[j].used = 1;
|
|
|
|
break;
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
static int topo_parse_link(struct media_v2_topology *topo, struct topo_param *param)
|
|
{
|
|
int i, j, k;
|
|
int ret = 0;
|
|
struct media_link *links = param->links;
|
|
struct media_entity *entity = param->entity;
|
|
|
|
for (i = 0; i < topo->num_links; i++) {
|
|
if ((links[i].flags & MEDIA_LNK_FL_LINK_TYPE) == MEDIA_LNK_FL_INTERFACE_LINK)
|
|
continue;
|
|
|
|
for (j = 0; j < ARRAY_SIZE(entity->pads); j++) {
|
|
if (!entity->pads[j].used)
|
|
continue;
|
|
|
|
if (links[i].source_id == entity->pads[j].id) {
|
|
for (k = 0; k < 16; k++) {
|
|
if (entity->pads[j].links[k])
|
|
continue;
|
|
|
|
entity->pads[j].links[k] = &links[i];
|
|
entity->pads[j].links[k]->src_pad = &entity->pads[j];
|
|
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (links[i].sink_id == entity->pads[j].id) {
|
|
for (k = 0; k < 16; k++) {
|
|
if (entity->pads[j].links[k])
|
|
continue;
|
|
|
|
entity->pads[j].links[k] = &links[i];
|
|
entity->pads[j].links[k]->sink_pad = &entity->pads[j];
|
|
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
static int media_create_sensor(struct media_v2_topology *topo,
|
|
struct sub_dev *sensor,
|
|
struct camera_dev *cam)
|
|
{
|
|
char *ptr = NULL;
|
|
struct topo_param param;
|
|
|
|
sensor->cam = cam;
|
|
|
|
param.ett_name = "-00";
|
|
param.entity = &sensor->entity;
|
|
param.dev = sensor;
|
|
param.links = cam->links;
|
|
|
|
topo_parse_entity(topo, ¶m);
|
|
if (param.entity->function != MEDIA_ENT_F_CAM_SENSOR) {
|
|
printf("Failed to match sensor entity\n");
|
|
return -ENODEV;
|
|
}
|
|
|
|
ptr = strstr(param.entity->name, " ");
|
|
if (ptr)
|
|
*ptr = '-';
|
|
|
|
topo_parse_pad(topo, ¶m);
|
|
topo_parse_link(topo, ¶m);
|
|
|
|
return topo_parse_devname(param.entity, sensor->devname, sizeof(sensor->devname));
|
|
}
|
|
|
|
static int media_create_sub_dev(struct media_v2_topology *topo,
|
|
struct sub_dev *sdev,
|
|
char *ett_name,
|
|
struct camera_dev *cam)
|
|
{
|
|
struct topo_param param = { 0 };
|
|
|
|
sdev->cam = cam;
|
|
|
|
param.ett_name = ett_name;
|
|
param.entity = &sdev->entity;
|
|
param.dev = sdev;
|
|
param.links = cam->links;
|
|
|
|
topo_parse_entity(topo, ¶m);
|
|
topo_parse_pad(topo, ¶m);
|
|
topo_parse_link(topo, ¶m);
|
|
|
|
return topo_parse_devname(param.entity, sdev->devname, sizeof(sdev->devname));
|
|
}
|
|
|
|
static int media_create_video_dev(struct media_v2_topology *topo,
|
|
struct video_dev *vdev,
|
|
char *ett_name,
|
|
struct camera_dev *cam)
|
|
{
|
|
struct topo_param param = { 0 };
|
|
|
|
vdev->cam = cam;
|
|
|
|
param.ett_name = ett_name;
|
|
param.entity = &vdev->entity;
|
|
param.dev = vdev;
|
|
param.links = cam->links;
|
|
|
|
topo_parse_entity(topo, ¶m);
|
|
topo_parse_pad(topo, ¶m);
|
|
topo_parse_link(topo, ¶m);
|
|
|
|
return topo_parse_devname(param.entity, vdev->devname, sizeof(vdev->devname));
|
|
}
|
|
|
|
static int media_create_camera(struct media_v2_topology *topo, struct camera_dev *cam)
|
|
{
|
|
struct media_v2_link *links = (void *)topo->ptr_links;
|
|
uint32_t i;
|
|
int ret;
|
|
|
|
if (topo->num_links > ARRAY_SIZE(cam->links)) {
|
|
printf("Error links array size\n");
|
|
return -EINVAL;
|
|
}
|
|
|
|
for (i = 0; i < topo->num_links; i++) {
|
|
cam->links[i].id = links[i].id;
|
|
cam->links[i].source_id = links[i].source_id;
|
|
cam->links[i].sink_id = links[i].sink_id;
|
|
cam->links[i].flags = links[i].flags;
|
|
cam->links[i].used = 1;
|
|
}
|
|
|
|
ret = media_create_sub_dev(topo, &cam->csiphy, CAM_CSI, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_sub_dev(topo, &cam->adap, CAM_ADAP, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_sub_dev(topo, &cam->ipp, CAM_IPP, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_sub_dev(topo, &cam->isp, CAM_ISP, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_sub_dev(topo, &cam->view_rsz, ISP_RSZ0, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_sub_dev(topo, &cam->cap_rsz, ISP_RSZ1, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_sub_dev(topo, &cam->record_rsz, ISP_RSZ2, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_sub_dev(topo, &cam->disp_rsz, ISP_RSZ3, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_video_dev(topo, &cam->stats, ISP_STATS, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_video_dev(topo, &cam->imp, ISP_IMP, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_video_dev(topo, &cam->param, ISP_PARAMS, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_video_dev(topo, &cam->raw, ISP_RAW, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_video_dev(topo, &cam->view, ISP_CAP0, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_video_dev(topo, &cam->cap, ISP_CAP1, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_video_dev(topo, &cam->record, ISP_CAP2, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_video_dev(topo, &cam->disp, ISP_CAP3, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_video_dev(topo, &cam->vc0, IPP_CAP0, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_video_dev(topo, &cam->vc1, IPP_CAP1, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_video_dev(topo, &cam->vc2, IPP_CAP2, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_video_dev(topo, &cam->vc3, IPP_CAP3, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
ret = media_create_sensor(topo, &cam->sensor, cam);
|
|
if (ret)
|
|
return ret;
|
|
|
|
cam->name = cam->sensor.entity.name;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int media_setup_link(struct camera_dev *cam, struct media_link *link, int flag)
|
|
{
|
|
int ret;
|
|
struct media_pad *src_pad = link->src_pad;
|
|
struct media_pad *sink_pad = link->sink_pad;
|
|
struct media_link_desc link_desc;
|
|
|
|
memset(&link_desc, 0, sizeof(link_desc));
|
|
|
|
if (link->flags & MEDIA_LNK_FL_IMMUTABLE)
|
|
return 0;
|
|
|
|
link_desc.source.entity = src_pad->entity->id;
|
|
link_desc.source.index = src_pad->index;
|
|
link_desc.source.flags = MEDIA_PAD_FL_SOURCE;
|
|
|
|
link_desc.sink.entity = sink_pad->entity->id;
|
|
link_desc.sink.index = sink_pad->index;
|
|
link_desc.sink.flags = MEDIA_PAD_FL_SINK;
|
|
|
|
link_desc.flags = (flag ? MEDIA_LNK_FL_ENABLED : 0);
|
|
|
|
ret = ioctl(cam->mfd, MEDIA_IOC_SETUP_LINK, &link_desc);
|
|
if (ret)
|
|
printf("Error to setup media link: %s\n", strerror(errno));
|
|
|
|
return ret;
|
|
}
|
|
|
|
int media_disable_entity_links(struct camera_dev *cam, struct media_entity *entity)
|
|
{
|
|
int i = 0, j = 0;
|
|
struct media_link *link = NULL;
|
|
|
|
for (i = 0; i < ARRAY_SIZE(entity->pads); i++) {
|
|
if (!entity->pads[i].used)
|
|
continue;
|
|
|
|
if (entity->pads[i].flags & MEDIA_PAD_FL_SOURCE)
|
|
continue;
|
|
|
|
for (j = 0; j < ARRAY_SIZE(entity->pads[i].links); j++) {
|
|
link = entity->pads[i].links[j];
|
|
if (!link)
|
|
break;
|
|
|
|
if (link->flags & MEDIA_LNK_FL_IMMUTABLE)
|
|
continue;
|
|
|
|
media_setup_link(cam, link, 0);
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int media_parse_topo(char *name, struct camera_dev *cam)
|
|
{
|
|
int ret = 0;
|
|
int fd = -1;
|
|
char *ptr = NULL;
|
|
uint64_t version = -1;
|
|
struct media_device_info info;
|
|
struct media_v2_topology topo;
|
|
struct media_v2_entity *entity = NULL;
|
|
struct media_v2_interface *inface = NULL;
|
|
struct media_v2_link *links = NULL;
|
|
struct media_v2_pad *pads = NULL;
|
|
|
|
fd = open(name, O_RDWR);
|
|
if (fd < 0) {
|
|
printf("failed to open %s: %s\n", name, strerror(errno));
|
|
return -ENOENT;
|
|
}
|
|
|
|
ret = ioctl(fd, MEDIA_IOC_DEVICE_INFO, &info);
|
|
if (ret < 0) {
|
|
close(fd);
|
|
cam->status = STATUS_UNPLUG;
|
|
printf("failed to get info\n");
|
|
return ret;
|
|
}
|
|
|
|
printf("media: %s, model %s, driver %s, version 0x%x\n",
|
|
name, info.model, info.driver, info.media_version);
|
|
|
|
ptr = strstr(info.model, CAM_NAME);
|
|
if (ptr == NULL) {
|
|
close(fd);
|
|
cam->status = STATUS_UNPLUG;
|
|
return -EINVAL;
|
|
}
|
|
|
|
memset(&topo, 0, sizeof(topo));
|
|
|
|
while (1) {
|
|
topo.ptr_entities = (uint64_t)entity;
|
|
topo.ptr_interfaces = (uint64_t)inface;
|
|
topo.ptr_links = (uint64_t)links;
|
|
topo.ptr_pads = (uint64_t)pads;
|
|
ret = ioctl(fd, MEDIA_IOC_G_TOPOLOGY, &topo);
|
|
if (ret < 0) {
|
|
printf("Failed to get topo: %s\n", strerror(errno));
|
|
close(fd);
|
|
cam->status = STATUS_UNPLUG;
|
|
return -EINVAL;
|
|
}
|
|
|
|
if (version == topo.topology_version)
|
|
break;
|
|
|
|
entity = malloc(topo.num_entities * sizeof(*entity));
|
|
inface = malloc(topo.num_interfaces * sizeof(*inface));
|
|
links = malloc(topo.num_links * sizeof(*links));
|
|
pads = malloc(topo.num_pads * sizeof(*pads));
|
|
|
|
version = topo.topology_version;
|
|
}
|
|
dev_rdev_cache_build();
|
|
ret = media_create_camera(&topo, cam);
|
|
if (ret) {
|
|
printf("Failed to create camera: %d\n", ret);
|
|
cam->status = STATUS_UNPLUG;
|
|
return ret;
|
|
}
|
|
|
|
if (entity)
|
|
free(entity);
|
|
if (inface)
|
|
free(inface);
|
|
if (links)
|
|
free(links);
|
|
if (pads)
|
|
free(pads);
|
|
|
|
cam->mfd = fd;
|
|
cam->status = STATUS_PLUG;
|
|
|
|
return 0;
|
|
}
|
|
|
|
void media_close_dev(struct camera_dev *cam)
|
|
{
|
|
close(cam->mfd);
|
|
}
|