537 lines
12 KiB
C
537 lines
12 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 "local_media.h"
|
|
|
|
static void *topo_find_iface(void *topology, uint32_t ett_id)
|
|
{
|
|
int i = 0;
|
|
uint32_t iface_id = 0;
|
|
struct media_v2_topology *topo = topology;
|
|
struct media_v2_link *links = (void *)topo->ptr_links;
|
|
struct media_v2_interface *ifaces = (void *)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)
|
|
{
|
|
int fd = -1;
|
|
long rsize = 0;
|
|
char uevent[64];
|
|
char *dir = "/dev/";
|
|
char *prefix = "DEVNAME=";
|
|
char buff[128] = {'\0'};
|
|
char *dev = NULL;
|
|
char *str = NULL;
|
|
|
|
if (!entity || !devname) {
|
|
printf("Error input param\n");
|
|
return -EINVAL;
|
|
}
|
|
|
|
snprintf(uevent, sizeof(uevent), "/sys/dev/char/%u:%u/uevent", entity->major, entity->minor);
|
|
fd = open(uevent, O_RDONLY);
|
|
if (fd < 0) {
|
|
printf("Failed to open %s\n", uevent);
|
|
return -ENODEV;
|
|
}
|
|
|
|
rsize = read(fd, buff, sizeof(buff));
|
|
if (rsize <= 0) {
|
|
close(fd);
|
|
printf("Error to read %s: %s\n", uevent, strerror(errno));
|
|
return -EINVAL;
|
|
}
|
|
|
|
close(fd);
|
|
|
|
str = strstr(buff, prefix);
|
|
if (!str) {
|
|
printf("Error to find DEVNAME=\n");
|
|
return -ENODEV;
|
|
}
|
|
|
|
dev = str + strlen(prefix);
|
|
snprintf(devname, strlen(dir) + strlen(dev), "%s%s", dir, dev);
|
|
|
|
return 0;
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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)
|
|
return -ENOENT;
|
|
|
|
ret = ioctl(fd, MEDIA_IOC_DEVICE_INFO, &info);
|
|
if (ret < 0) {
|
|
close(fd);
|
|
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);
|
|
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);
|
|
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;
|
|
}
|
|
|
|
ret = media_create_camera(&topo, cam);
|
|
if (ret) {
|
|
printf("Failed to create camera: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
if (entity)
|
|
free(entity);
|
|
if (inface)
|
|
free(inface);
|
|
if (links)
|
|
free(links);
|
|
if (pads)
|
|
free(pads);
|
|
|
|
cam->mfd = fd;
|
|
|
|
return 0;
|
|
}
|
|
|
|
void media_close_dev(struct camera_dev *cam)
|
|
{
|
|
close(cam->mfd);
|
|
}
|