BPI/BPI-CM9-Linux
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
818cb714e3504578f68bc0a214df1ba8a0ec1edd
BPI-CM9-Linux/aml-comp/vendor/amlogic/mediactrlsrc/camctrl/camctrl.cc
T
Qubot 818cb714e3 Import complete source tree
2026-08-24 10:03:24 +08:00

1169 lines
34 KiB
C++
Raw Blame History

/*
* Copyright (c) 2019 Amlogic, Inc. All rights reserved.
*
* This source code is subject to the terms and conditions defined in below
* which is part of this source code package.
*
* Description: Mediactrlsrc
*/
// Copyright (C) 2019 Amlogic, Inc. All rights reserved.
//
// All information contained herein is Amlogic confidential.
//
// This software is provided to you pursuant to Software License
// Agreement (SLA) with Amlogic Inc ("Amlogic"). This software may be
// used only in accordance with the terms of this agreement.
//
// Redistribution and use in source and binary forms, with or without
// modification is strictly prohibited without prior written permission
// from Amlogic.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define _GNU_SOURCE
#include <stdlib.h>
#include <stdio.h>
#include <stddef.h>
#include <stdbool.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <errno.h>
#include <string.h>
#include <unistd.h>
#include <ctype.h>
#include <pthread.h>
#include <dlfcn.h>
#include <poll.h>
#include <vector>
#include "mediactrl_common.h"
#include "mediactrl_log.h"
#include "mediactl.h"
#include "v4l2subdev.h"
#include "v4l2videodev.h"
#include "mediaApi.h"
#include "lens_config.h"
#include "staticPipe.h"
#include "ispMgr.h"
// #define PRINT_FPS
static int connected_sockfd = -1;
static int listen_fd = -1;
static int mFlushFd[2] = {-1, -1};
static char *media_device_name = NULL;
static const char *server_socket = DEFAULT_SERVER_SOCKET0;
static struct media_device *media_dev = NULL;
static int camera_num = 1;
/* media stream */
media_stream_t v4l2_media_stream;
//#define WDR_ENABLE
#define V4L2_META_AML_ISP_CONFIG v4l2_fourcc('A', 'C', 'F', 'G') /* Aml isp param config */
#define V4L2_META_AML_ISP_STATS v4l2_fourcc('A', 'S', 'T', 'S') /* Aml isp statistics */
#define DEFAULT_BUFFER_NUM 4
typedef struct v4l2buffer_info {
void *pstart;
int length;
} v4l2buffer_info_t;
enum MEDIACTRLSRC_STREAM {
MEDIACTRLSRC_STREAM_OUTPUT0 = 0, /* Not Used */
MEDIACTRLSRC_STREAM_RAW, /* Not Used */
MEDIACTRLSRC_STREAM_STATISTICS,
MEDIACTRLSRC_STREAM_MAX
};
pthread_t tid[MEDIACTRLSRC_STREAM_MAX];
struct thread_info {
pthread_t p_tid;
uint32_t status;
/* sem_t p_sem; */
/* aml isp algorithm related */
aisp_calib_info_t calib;
AML_ALG_CTX_S pstAlgCtx;
};
struct thread_param {
//v4l2buffer_info_t *buffer_isp_output0[DEFAULT_BUFFER_NUM]; /* reserved */
v4l2buffer_info_t buffer_isp_stats[DEFAULT_BUFFER_NUM];
v4l2buffer_info_t buffer_isp_param;
struct sensorConfig *sensor_cfg;
struct lensConfig *lens_cfg;
/* format related */
uint32_t width;
uint32_t height;
// uint32_t pixformat;
uint32_t fmt_code;
uint32_t wdr_mode;
uint32_t capture_count;
int fps;
pthread_mutex_t mutex;
pthread_cond_t cond;
pthread_t process_socket_tid;
bool streaming;
struct thread_info info;
};
int pollDevices(const std::vector<struct media_entity *> &devices,
std::vector<struct media_entity *> &activeDevices,
std::vector<struct media_entity *> &inactiveDevices,
int timeOut, int flushFd = -1,
int events = POLLPRI | POLLIN | POLLERR);
/*
* definition of tparam (struct thread_param)
* set default value
*/
static struct thread_param tparam = {
.buffer_isp_stats={0},
.buffer_isp_param={0},
.sensor_cfg = NULL,
.lens_cfg = NULL,
.width = 3840,
.height = 1920,
// .pixformat = V4L2_PIX_FMT_NV12, /* V4L2_PIX_FMT_NV12, V4L2_PIX_FMT_Y12, V4L2_PIX_FMT_SRGGB12 */
#ifdef WDR_ENABLE
.fmt_code = MEDIA_BUS_FMT_SBGGR10_1X10, /* MEDIA_BUS_FMT_SRGGB12_1X12 */
.wdr_mode = WDR_MODE_2To1_FRAME, /* WDR_MODE_2To1_LINE */
#else
.fmt_code = MEDIA_BUS_FMT_SBGGR10_1X10, /* MEDIA_BUS_FMT_SRGGB12_1X12 */
.wdr_mode = WDR_MODE_NONE,
#endif
.capture_count = 0,
.fps = 0,
.mutex = PTHREAD_MUTEX_INITIALIZER,
.cond = PTHREAD_COND_INITIALIZER,
.process_socket_tid = 0,
.streaming = false,
.info = {0},
};
/*
* check video device capability
*/
static int check_capability(struct media_entity *entity) {
struct v4l2_capability v4l2_cap;
memset (&v4l2_cap, 0, sizeof (struct v4l2_capability));
int ret = v4l2_video_get_capability(entity, &v4l2_cap);
if (ret < 0) {
log_error("get entity[%s] -> video[%s] capability failed",
entity->info.name,
entity->devname);
return -1;
}
log_debug("entity[%s] -> video[%s], cap.driver:%s, capabilities:0x%x, device_caps:0x%x",
entity->info.name,
entity->devname,
v4l2_cap.driver,
v4l2_cap.capabilities,
v4l2_cap.device_caps);
return 0;
}
static int media_set_wdr_mode(media_stream_t *stream, uint32_t wdr_mode) {
int ret = -1;
media_set_wdrMode(stream, 0);
ret = media_set_wdrMode(stream, wdr_mode);
return ret;
#if 0
/* sensor wdr mode */
if (wdr_mode != ISP_SDR_DCAM_MODE) {
ret = v4l2_subdev_set_wdr(stream->sensor.entity, wdr_mode);
if (ret < 0) {
log_error("set sensor wdr mode failed");
return ret;
}
}
/* isp-adapter wdr mode */
ret = v4l2_subdev_set_wdr(stream->isp_adapter.entity, wdr_mode);
if (ret < 0) {
log_error("set isp-adapter wdr mode failed");
return ret;
}
/* isp-core wdr mode */
ret = v4l2_subdev_set_wdr(stream->isp_core.entity, wdr_mode);
if (ret < 0) {
log_error("set isp-core wdr mode failed");
return ret;
}
log_debug("set wdr mode successfully");
return ret;
#endif
}
static int media_stream_init(media_stream_t *stream,
struct media_device *media_dev) {
mediaStreamInit(stream, media_dev);
fetchPipeMaxResolution(
&v4l2_media_stream,
&tparam.width,
&tparam.height
);
log_debug("camera num %d", camera_num);
if (1 == camera_num) {
tparam.fmt_code = MEDIA_BUS_FMT_SRGGB12_1X12;
tparam.wdr_mode = WDR_MODE_NONE;
} else if (2 == camera_num) {
tparam.fmt_code = MEDIA_BUS_FMT_SRGGB12_1X12;
tparam.wdr_mode = ISP_SDR_DCAM_MODE;
} else {
log_error("only support 1 or 2 camera");
return 0;
}
/* set wdr mode */
media_set_wdr_mode(stream, tparam.wdr_mode);
/* TODO */
return 0;
}
/*
* set subdev(entity pad) format
* include sensor, isp-csiphy, isp-adapter, isp-core
*/
static int set_subdev_pad_format(media_stream_t *stream,
stream_configuration_t *cfg) {
int ret = -1;
struct v4l2_mbus_framefmt mbus_format;
enum v4l2_subdev_format_whence which = V4L2_SUBDEV_FORMAT_ACTIVE;
mbus_format.width = cfg->format.width;
mbus_format.height = cfg->format.height;
mbus_format.code = cfg->format.code;
/* sensor source pad[0] format */
ret = v4l2_subdev_set_format(stream->sensor_ent,
&mbus_format, 0, which);
if (ret < 0) {
log_error("set subdev sensor pad[0] format failed");
return ret;
}
/* isp-csiphy sink & source pad[0, 1] format */
ret = v4l2_subdev_set_format(stream->csiphy_ent,
&mbus_format, 0, which);
if (ret < 0) {
log_error("set subdev isp-ciphy pad[0] format failed");
return ret;
}
ret = v4l2_subdev_set_format(stream->csiphy_ent,
&mbus_format, 1, which);
if (ret < 0) {
log_error("set subdev isp-csiphy pad[1] format failed");
return ret;
}
/* isp-adapter sink & source pad[0, 1] format */
ret = v4l2_subdev_set_format(stream->adap_ent,
&mbus_format, 0, which);
if (ret < 0) {
log_error("set subdev isp-adapter pad[0] format failed");
return ret;
}
ret = v4l2_subdev_set_format(stream->adap_ent,
&mbus_format, 1, which);
if (ret < 0) {
log_error("set subdev isp-adapter pad[1] format failed");
return ret;
}
/* isp-core sink pad[0] format */
ret = v4l2_subdev_set_format(stream->isp_ent,
&mbus_format, 0, which);
if (ret < 0) {
log_error("set subdev isp-core pad[0] format failed");
return ret;
}
return ret;
}
/*
* link and activate subdev(entity pad) which
* includes sensor, isp-csiphy, isp-adapter, isp-core
*/
#define SENSOR_SOURCE_PAD_INDEX 0
#define SINK_PAD_INDEX 0
#define SOURCE_PAD_INDEX 1
static int link_and_activate_subdev(media_stream_t *stream) {
int ret = -1;
struct media_pad *sink_pad = NULL;
struct media_pad *source_pad = NULL;
int flag = MEDIA_LNK_FL_ENABLED; /* The link is enabled and can be used to transfer media data */
/* source pad[0] sensor --> sink pad[0] isp-csiphy */
source_pad = (struct media_pad*)media_entity_get_pad(stream->sensor_ent, SENSOR_SOURCE_PAD_INDEX);
if (!source_pad) {
log_error("get sensor source pad[0] failed");
return -1;
}
sink_pad = (struct media_pad*)media_entity_get_pad(stream->csiphy_ent, SINK_PAD_INDEX);
if (!sink_pad) {
log_error("get isp-csiphy sink pad[0] failed");
return -1;
}
ret = media_setup_link(media_dev, source_pad, sink_pad, flag);
if (ret != 0) {
log_error("link sensor to isp-csiphy failed");
return ret;
}
/* source pad[1] isp-csiphy --> sink pad[0] isp-adapter */
source_pad = (struct media_pad*)media_entity_get_pad(stream->csiphy_ent, SOURCE_PAD_INDEX);
if (!source_pad) {
log_error("get isp-csiphy source pad[1] failed");
return -1;
}
sink_pad = (struct media_pad*)media_entity_get_pad(stream->adap_ent, SINK_PAD_INDEX);
if (!sink_pad) {
log_error("get isp-adapter sink pad[0] failed");
return -1;
}
ret = media_setup_link(media_dev, source_pad, sink_pad, flag);
if (ret != 0) {
log_error("link isp-csiphy to isp-adapter failed");
return ret;
}
/* source pad[1] isp-adapter --> sink pad[0] isp-core */
source_pad = (struct media_pad*)media_entity_get_pad(stream->adap_ent, SOURCE_PAD_INDEX);
if (!source_pad) {
log_error("get isp-adapter source pad[1] failed");
return -1;
}
sink_pad = (struct media_pad*)media_entity_get_pad(stream->isp_ent, SINK_PAD_INDEX);
if (!sink_pad) {
log_error("get isp-core sink pad[0] failed");
return -1;
}
ret = media_setup_link(media_dev, source_pad, sink_pad, flag);
if (ret != 0) {
log_error("link isp-adapter to isp-core failed");
return ret;
}
log_debug("link and activate subdev successfully");
return ret;
}
/*
* media stream config
* >1. setup necessary subdev's pad format
* >2. link and activate necessary subdev
*/
int media_stream_config(media_stream_t *stream) {
int ret = -1;
stream_configuration_t stream_cfg;
memset(&stream_cfg, 0, sizeof(stream_configuration_t));
stream_cfg.format.width = tparam.width;
stream_cfg.format.height = tparam.height;
// stream_cfg.format.fourcc = tparam.pixformat;
stream_cfg.format.code = tparam.fmt_code;;
stream_cfg.format.nplanes = 1;
ret = set_subdev_pad_format(stream, &stream_cfg);
if (ret < 0) {
log_error("set subdev format failed");
return ret;
}
ret = link_and_activate_subdev(stream);
if (ret < 0) {
log_error("link and activate subdev failed");
return ret;
}
log_debug("config media stream successfully");
return ret;
}
/*
* set isp-stats entity format (out: statistics)
*/
static int set_isp_stats_fmt(media_stream_t *stream,
stream_configuration_t *cfg) {
int ret = -1;
struct v4l2_format v4l2_fmt;
memset (&v4l2_fmt, 0, sizeof (struct v4l2_format));
v4l2_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l2_fmt.fmt.pix_mp.width = cfg->format.width;
v4l2_fmt.fmt.pix_mp.height = cfg->format.height;
v4l2_fmt.fmt.pix_mp.pixelformat = V4L2_META_AML_ISP_STATS;
v4l2_fmt.fmt.pix_mp.field = V4L2_FIELD_ANY;
ret = v4l2_video_set_format(stream->video_stats, &v4l2_fmt);
if (ret < 0) {
log_error("set isp-stats fmt failed, ret:%d", ret);
return ret;
}
return 0;
}
/*
* set isp-param entity format (in: parameter)
*/
static int set_isp_param_fmt(media_stream_t *stream,
stream_configuration_t *cfg) {
int ret = -1;
struct v4l2_format v4l2_fmt;
memset (&v4l2_fmt, 0, sizeof (struct v4l2_format));
v4l2_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l2_fmt.fmt.pix_mp.width = cfg->format.width;
v4l2_fmt.fmt.pix_mp.height = cfg->format.height;
v4l2_fmt.fmt.pix_mp.pixelformat = V4L2_META_AML_ISP_CONFIG;
v4l2_fmt.fmt.pix_mp.field = V4L2_FIELD_ANY;
ret = v4l2_video_set_format(stream->video_param, &v4l2_fmt);
if (ret < 0) {
log_error("set isp-param fmt failed, ret:%d", ret);
return ret;
}
return 0;
}
/* isp algorithm interface */
struct ispIF isp_alg_iface;
static int get_isp_alg_interface() {
void *handle;
handle = dlopen("/usr/lib/libispaml.so", RTLD_NOW);
if (!handle) {
printf("dlopen libispaml.so error: %s\n", dlerror());
return -1;
}
isp_alg_iface.alg2User = (isp_alg2user)dlsym(handle, "aisp_alg2user");
isp_alg_iface.alg2Kernel = (isp_alg2kernel)dlsym(handle, "aisp_alg2kernel");
isp_alg_iface.algEnable = (isp_enable)dlsym(handle, "aisp_enable");
isp_alg_iface.algDisable = (isp_disable)dlsym(handle, "aisp_disable");
isp_alg_iface.algFwInterface = (isp_fw_interface)dlsym(handle, "aisp_fw_interface");
printf("alg2User func addr: %p\n", isp_alg_iface.alg2User);
printf("alg2Kernel func addr: %p\n", isp_alg_iface.alg2Kernel);
printf("algEnable func addr: %p\n", isp_alg_iface.algEnable);
printf("algDisable func addr: %p\n", isp_alg_iface.algDisable);
printf("algFwInterface func addr: %p\n", isp_alg_iface.algFwInterface);
return 0;
}
/*
* init isp algorithm thread related parameter
* >1. set isp-stats and isp-param entity format
* >2. request and map buffer respectively
* >3. first queue to clean buffer before streamon
* >4. streamon
*/
static void isp_alg_param_init() {
int ret = -1;
mFlushFd[0] = -1;
mFlushFd[1] = -1;
ret = pipe(mFlushFd);
if (ret < 0) {
log_error("Failed to create Flush pipe: %s", strerror(errno));
return;
}
/**
* make the reading end of the pipe non blocking.
* This helps during flush to read any information left there without
* blocking
*/
ret = fcntl(mFlushFd[0], F_SETFL, O_NONBLOCK);
if (ret < 0) {
log_error("Fail to set flush pipe flag: %s", strerror(errno));
return;
}
ret = fcntl(mFlushFd[1], F_SETFL, O_NONBLOCK);
if (ret < 0) {
log_error("Fail to set flush pipe flag: %s", strerror(errno));
return;
}
check_capability(v4l2_media_stream.video_stats);
check_capability(v4l2_media_stream.video_param);
stream_configuration_t common_cfg;
common_cfg.format.width = 1024;
common_cfg.format.height = 256;
common_cfg.format.nplanes = 1;
set_isp_stats_fmt(&v4l2_media_stream, &common_cfg);
set_isp_param_fmt(&v4l2_media_stream, &common_cfg);
/* isp-stats & isp-param request buffers */
struct v4l2_requestbuffers v4l2_rb;
memset (&v4l2_rb, 0, sizeof (struct v4l2_requestbuffers));
v4l2_rb.count = DEFAULT_BUFFER_NUM;
v4l2_rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l2_rb.memory = V4L2_MEMORY_MMAP;
ret = v4l2_video_req_bufs(v4l2_media_stream.video_stats, &v4l2_rb);
if (ret < 0) {
log_error("isp-stats request buffer error");
return;
}
memset (&v4l2_rb, 0, sizeof (struct v4l2_requestbuffers));
v4l2_rb.count = 1;
v4l2_rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l2_rb.memory = V4L2_MEMORY_MMAP;
ret = v4l2_video_req_bufs(v4l2_media_stream.video_param, &v4l2_rb);
if (ret < 0) {
log_error("isp-param request buffer error");
return;
}
/* map isp-stats & isp-param buffers */
struct v4l2_buffer v4l2_buf;
for (int i = 0; i < DEFAULT_BUFFER_NUM; i++) {
memset (&v4l2_buf, 0, sizeof (struct v4l2_buffer));
v4l2_buf.index = i;
v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l2_buf.memory = V4L2_MEMORY_MMAP;
ret = v4l2_video_query_buf(v4l2_media_stream.video_stats, &v4l2_buf);
if (ret < 0) {
log_error("isp-stats query buffer error, ret: %d", ret);
return;
}
if (v4l2_buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
tparam.buffer_isp_stats[i].length = v4l2_buf.length;
log_debug("isp stats query buffer, length: %u, offset: %d",
v4l2_buf.length, v4l2_buf.m.offset);
tparam.buffer_isp_stats[i].pstart = mmap (NULL, v4l2_buf.length,
PROT_READ | PROT_WRITE, MAP_SHARED, v4l2_media_stream.video_stats->fd, v4l2_buf.m.offset);
if (tparam.buffer_isp_stats[i].pstart == MAP_FAILED) {
log_error("isp stats map buffer error");
return;
}
}
}
memset (&v4l2_buf, 0, sizeof (struct v4l2_buffer));
v4l2_buf.index = 0;
v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l2_buf.memory = V4L2_MEMORY_MMAP;
ret = v4l2_video_query_buf(v4l2_media_stream.video_param, &v4l2_buf);
if (ret < 0) {
log_error("isp-param query buffer error, ret: %d", ret);
return;
}
if (v4l2_buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
tparam.buffer_isp_param.length = v4l2_buf.length;
log_debug("isp param query buffer, length: %u, offset: %d",
v4l2_buf.length, v4l2_buf.m.offset);
tparam.buffer_isp_param.pstart = mmap (NULL, v4l2_buf.length,
PROT_READ | PROT_WRITE, MAP_SHARED, v4l2_media_stream.video_param->fd, v4l2_buf.m.offset);
if (tparam.buffer_isp_param.pstart == MAP_FAILED) {
log_error("isp stats map buffer error");
return;
}
}
/* TODO:
* add interface of aml isp algorithm
*/
/* Initialize algorithm related */
char alg_init[256 * 1024];
if (-1 == get_isp_alg_interface()) {
log_error("get isp algorithm interface failed");
return;
}
tparam.sensor_cfg = matchSensorConfigByStream(&v4l2_media_stream);
if (NULL == tparam.sensor_cfg) {
log_error("match sensor config failed");
return;
}
tparam.lens_cfg = matchLensConfigByStream(&v4l2_media_stream);
if (tparam.lens_cfg != NULL) {
lens_set_entity(tparam.lens_cfg, v4l2_media_stream.lens_ent);
lens_control_cb(tparam.lens_cfg, &tparam.info.pstAlgCtx.stLensFunc);
}
#ifdef WDR_ENABLE
cmos_set_sensor_entity(tparam.sensor_cfg, v4l2_media_stream.sensor_ent, 1);
#else
cmos_set_sensor_entity(tparam.sensor_cfg, v4l2_media_stream.sensor_ent, 0);
#endif
cmos_sensor_control_cb(tparam.sensor_cfg, &tparam.info.pstAlgCtx.stSnsExp);
cmos_get_sensor_calibration(tparam.sensor_cfg, v4l2_media_stream.sensor_ent, &tparam.info.calib);
(isp_alg_iface.algEnable)(0, &tparam.info.pstAlgCtx, &tparam.info.calib);
memset(alg_init, 0, sizeof(alg_init));
(isp_alg_iface.alg2User)(0, alg_init);
(isp_alg_iface.alg2Kernel)(0, tparam.buffer_isp_param.pstart);
/* first queue isp-stats & isp-param buffers to clean */
for (int i = 0; i < DEFAULT_BUFFER_NUM; i++) {
memset (&v4l2_buf, 0, sizeof (struct v4l2_buffer));
v4l2_buf.index = i;
v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l2_buf.memory = V4L2_MEMORY_MMAP;
ret = v4l2_video_q_buf(v4l2_media_stream.video_stats, &v4l2_buf);
if (ret < 0) {
log_error("isp-stats first queue buffer error, ret: %d, index: %d", ret, i);
return;
}
}
memset (&v4l2_buf, 0, sizeof (struct v4l2_buffer));
v4l2_buf.index = 0;
v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l2_buf.memory = V4L2_MEMORY_MMAP;
ret = v4l2_video_q_buf(v4l2_media_stream.video_param, &v4l2_buf);
if (ret < 0) {
log_error("isp-param first queue buffer error, ret: %d", ret);
return;
}
/* streamon isp-stats & isp-param */
ret = v4l2_video_stream_on(v4l2_media_stream.video_stats,
V4L2_BUF_TYPE_VIDEO_CAPTURE);
if (ret < 0) {
log_error("isp-stats streamon error, ret: %d", ret);
return;
}
ret = v4l2_video_stream_on(v4l2_media_stream.video_param,
V4L2_BUF_TYPE_VIDEO_CAPTURE);
if (ret < 0) {
log_error("isp-stats streamon error, ret: %d", ret);
return;
}
log_debug("Finish initializing amlgorithm parameter ...");
}
#ifdef PRINT_FPS
static int64_t get_current_time_msec(void) {
struct timeval tv;
gettimeofday(&tv, NULL);
return tv.tv_sec * 1000 + tv.tv_usec / 1000;
}
#endif
/**
* check fd whether is readable
**/
int check_fd(int fd, int timeout_us) {
if (timeout_us >= 0) {
struct timeval tv;
struct timeval *ptv;
if (timeout_us > 0) {
tv.tv_sec = timeout_us / 1000000;
tv.tv_usec = timeout_us % 1000000;
ptv = &tv;
} else {
ptv = NULL;
}
fd_set rfds;
FD_ZERO(&rfds);
FD_SET(fd, &rfds);
int retval = select(fd + 1, &rfds, NULL, NULL, ptv);
if (retval <= 0) {
log_error("select fail %d, %d %s", retval, errno, strerror(errno));
return -1;
}
if (fd > 0 && FD_ISSET(fd, &rfds)) {
return 0;
} else {
return -1;
}
}
return -1;
}
int pollDevices(const std::vector<struct media_entity *> &devices,
std::vector<struct media_entity *> &activeDevices,
std::vector<struct media_entity *> &inactiveDevices,
int timeOut, int flushFd, int events) {
int numFds = devices.size();
int totalNumFds = (flushFd != -1) ? numFds + 1 : numFds;
struct pollfd pollFds[totalNumFds];
int ret = 0;
for (int i = 0; i < numFds; i++) {
pollFds[i].fd = devices[i]->fd;
pollFds[i].events = events | POLLERR; // we always poll for errors, asked or not
pollFds[i].revents = 0;
}
if (flushFd != -1) {
pollFds[numFds].fd = flushFd;
pollFds[numFds].events = POLLPRI | POLLIN;
pollFds[numFds].revents = 0;
}
ret = poll(pollFds, totalNumFds, timeOut);
if (ret <= 0) {
for (uint32_t i = 0; i < devices.size(); i++) {
log_error("Device %s poll failed (%s)", devices[i]->info.name,
(ret == 0) ? "timeout" : "error");
if (pollFds[i].revents & POLLERR) {
log_error("%s: device %s received POLLERR", __FUNCTION__, devices[i]->info.name);
}
}
return ret;
}
activeDevices.clear();
inactiveDevices.clear();
//check first the flush
if (flushFd != -1) {
if ((pollFds[numFds].revents & POLLIN) || (pollFds[numFds].revents & POLLPRI)) {
char dummy;
read(flushFd, &dummy, 1);
log_debug("%s: Poll returning from flush", __FUNCTION__);
return ret;
}
}
// check other active devices.
for (int i = 0; i < numFds; i++) {
if (pollFds[i].revents & POLLERR) {
log_error("%s: received POLLERR", __FUNCTION__);
return -1;
}
// return nodes that have data available
if (pollFds[i].revents & events) {
activeDevices.push_back(devices[i]);
} else
inactiveDevices.push_back(devices[i]);
}
return ret;
}
/**
* isp algorithm process one
**/
static void isp_alg_process_one(struct thread_param *tparam) {
// log_debug("enter isp_alg_process_one");
//polling for q_buf & dq_buf
int ret = -1;
int idx = -1;
std::vector<struct media_entity *> mPollingDevices;
std::vector<struct media_entity *> mActiveDevices;
std::vector<struct media_entity *> mInactiveDevices;
mPollingDevices.push_back(v4l2_media_stream.video_param);
mPollingDevices.push_back(v4l2_media_stream.video_stats);
do {
ret = pollDevices(mPollingDevices, mActiveDevices,
mInactiveDevices, 500, mFlushFd[0]);
if (mInactiveDevices.size() > 0) {
mPollingDevices = mInactiveDevices;
// log_info("not all device is ready, continue polling");
for (int i = 0; i < mInactiveDevices.size(); ++i)
// log_info("InactiveDevices %d: %s", i, mInactiveDevices[i]->info.name);
continue;
} else if (mActiveDevices.size() > 0 && mInactiveDevices.size() == 0) {
// log_info("all device is ready");
} else {
log_info("return from flush or error");
return;
}
struct v4l2_buffer v4l2_buf_video_statistics;
memset (&v4l2_buf_video_statistics, 0, sizeof (struct v4l2_buffer));
v4l2_buf_video_statistics.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l2_buf_video_statistics.memory = V4L2_MEMORY_MMAP;
// log_debug("enter v4l2_video_dq_buf video_stats");
ret = v4l2_video_dq_buf(
v4l2_media_stream.video_stats,
&v4l2_buf_video_statistics
);
// log_debug("exit v4l2_video_dq_buf video_stats");
if (ret < 0) {
log_error("dequeue video statistics buffer failed");
continue;
}
struct v4l2_buffer v4l2_buf_video_param;
memset (&v4l2_buf_video_param, 0, sizeof (struct v4l2_buffer));
v4l2_buf_video_param.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l2_buf_video_param.memory = V4L2_MEMORY_MMAP;
// log_debug("enter v4l2_video_dq_buf video_param");
ret = v4l2_video_dq_buf(
v4l2_media_stream.video_param,
&v4l2_buf_video_param
);
// log_debug("exit v4l2_video_dq_buf video_param");
if (ret < 0) {
log_error("dequeue video param buffer failed");
continue;
}
idx = v4l2_buf_video_statistics.index;
(isp_alg_iface.alg2User)(0, tparam->buffer_isp_stats[idx].pstart);
(isp_alg_iface.alg2Kernel)(0, tparam->buffer_isp_param.pstart);
ret = v4l2_video_q_buf(
v4l2_media_stream.video_stats,
&v4l2_buf_video_statistics
);
// log_debug("exit v4l2_video_q_buf video_stats");
if (ret < 0) {
log_error("queue video statistics buffer failed");
break;
}
// log_debug("enter v4l2_video_q_buf video_param");
ret = v4l2_video_q_buf(
v4l2_media_stream.video_param,
&v4l2_buf_video_param
);
// log_debug("exit v4l2_video_q_buf video_param");
if (ret < 0) {
log_error("queue video param buffer failed");
break;
}
break;
} while(1);
// log_debug("exit isp_alg_process_one");
return;
}
/**
* isp algorithm thread
**/
static void *isp_alg_thread(void *arg) {
struct thread_param *tparam = (struct thread_param *)arg;
/**
* use isp algorithm to process
**/
log_debug("tparam->streaming = %s", tparam->streaming ? "true" : "false");
pthread_mutex_lock(&tparam->mutex);
if (!tparam->streaming) {
pthread_cond_wait(&tparam->cond, &tparam->mutex);
}
// log_debug("tparam->streaming = %s", tparam->streaming ? "true" : "false");
pthread_mutex_unlock(&tparam->mutex);
// log_debug("tparam->streaming = %s", tparam->streaming ? "true" : "false");
while (true) {
pthread_mutex_lock(&tparam->mutex);
// log_debug("tparam->streaming = %s", tparam->streaming ? "true" : "false");
if (tparam->streaming) {
pthread_mutex_unlock(&tparam->mutex);
// log_debug("tparam->streaming = %s", tparam->streaming ? "true" : "false");
isp_alg_process_one(tparam);
// log_debug("tparam->streaming = %s", tparam->streaming ? "true" : "false");
} else {
pthread_mutex_unlock(&tparam->mutex);
log_debug("streamoff ...");
break;
}
}
/* streamoff and unmap isp-stats & isp-param buffers */
int ret = -1;
ret = v4l2_video_stream_off(v4l2_media_stream.video_stats,V4L2_BUF_TYPE_VIDEO_CAPTURE);
if (ret < 0) {
log_error("[stats] error: streamoff");
return NULL;
}
log_debug("v4l2_video_stream_off video_stats");
ret = v4l2_video_stream_off(v4l2_media_stream.video_param,V4L2_BUF_TYPE_VIDEO_CAPTURE);
if (ret < 0) {
log_error("[video_param] error: streamoff");
return NULL;
}
log_debug("enter v4l2_video_stream_off video_param");
(isp_alg_iface.algDisable)(0);
/* isp-stats & isp-param request buffers */
struct v4l2_requestbuffers v4l2_rb;
memset (&v4l2_rb, 0, sizeof (struct v4l2_requestbuffers));
v4l2_rb.count = 0;
v4l2_rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l2_rb.memory = V4L2_MEMORY_MMAP;
ret = v4l2_video_req_bufs(v4l2_media_stream.video_stats, &v4l2_rb);
if (ret < 0) {
log_error("[stats] error: request buffer.");
return NULL;
}
for (int i = 0; i < DEFAULT_BUFFER_NUM; i++) {
munmap(tparam->buffer_isp_stats[i].pstart,
tparam->buffer_isp_stats[i].length);
}
memset (&v4l2_rb, 0, sizeof (struct v4l2_requestbuffers));
v4l2_rb.count = 0;
v4l2_rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l2_rb.memory = V4L2_MEMORY_MMAP;
ret = v4l2_video_req_bufs(v4l2_media_stream.video_param, &v4l2_rb);
if (ret < 0) {
log_error("[video_param] error: request buffer.");
return NULL;
}
munmap(tparam->buffer_isp_param.pstart,
tparam->buffer_isp_param.length);
log_debug("exit isp_alg_thread");
return NULL;
}
static void *process_socket_thread(void *arg) {
struct thread_param *tparam = (struct thread_param *)arg;
int r;
char recv_buffer[32] = {0};
while (true) {
if (check_fd(connected_sockfd, 0) == 0) {
r = TEMP_FAILURE_RETRY(recv(connected_sockfd, recv_buffer, sizeof(recv_buffer), 0));
log_debug("recv_buffer:%s",recv_buffer);
if ((r == 0) || (r == -1L) || (strcmp("streamoff", recv_buffer) == 0)) {
pthread_mutex_lock(&tparam->mutex);
tparam->streaming = false;
pthread_cond_signal(&tparam->cond);
pthread_mutex_unlock(&tparam->mutex);
char dummy = 'x';
write(mFlushFd[1], &dummy, 1);
log_debug("receive streamoff notification or TEMP_FAILURE_RETRY 0/-1L :%d",r);
break;
}
if (strcmp("streamon", recv_buffer) == 0) {
pthread_mutex_lock(&tparam->mutex);
tparam->streaming = true;
pthread_cond_signal(&tparam->cond);
pthread_mutex_unlock(&tparam->mutex);
log_debug("receive streamon notification");
} else {
log_warn("not supported message ...");
continue;
}
}
}
log_debug("exit process_socket_thread");
return NULL;
}
static void parse_opt(int argc, char *argv[]) {
int opt;
while ((opt = getopt(argc, argv, "m:c:t:s:")) != -1) {
switch (opt) {
case 'm': /* media device name (/dev/media0) */
media_device_name = optarg;
// if (strstr(media_device_name, "media0")) {
// server_socket = DEFAULT_SERVER_SOCKET0;
// } else if (strstr(media_device_name, "media1")) {
// server_socket = DEFAULT_SERVER_SOCKET1;
// }
log_debug("media device name: %s", media_device_name);
break;
case 'c': /* camera number (0,1) */
camera_num = atoi(optarg);
break;
case 't':
break;
case 's':
server_socket = optarg;
log_debug("Server socket: %s",server_socket);
break;
default:
exit(1);
}
}
}
static void Signalhandler(int sig)
{
log_debug("enter camctrl Signalhandler: %d",sig);
char streamoff_buffer[32] = {0};
strcpy(streamoff_buffer, "streamoff_done");
int r = TEMP_FAILURE_RETRY(send(connected_sockfd, streamoff_buffer, strlen(streamoff_buffer), 0));
if (r < 0) {
log_error("send streamoff_buffer, failed");
}
close(listen_fd);
close(connected_sockfd);
unlink(server_socket);
if (mFlushFd[1] != -1 || mFlushFd[0] != -1) {
close(mFlushFd[0]);
close(mFlushFd[1]);
mFlushFd[0] = -1;
mFlushFd[1] = -1;
}
log_debug("exit camctrl Signalhandler: %d",sig);
exit(0);
}
int main(int argc, char *argv[]) {
signal(SIGINT, SIG_IGN); // Ignore the SIGINT signal
// signal(SIGTERM, Signalhandler);
// signal(SIGTERM, SIG_IGN); // Ignore the SIGTERM signal
log_debug("[%s:%s:%d]\n", __FILE__, __func__, __LINE__);
parse_opt(argc, argv);
/**
* media ctrl progress:
* open media device and enumerate entities (In general: /dev/media0)
* get video device name through entity name
*
**/
media_dev = media_device_new(media_device_name);
if (NULL == media_dev) {
log_debug("new media device error");
return -1;
}
if (0 != media_device_enumerate(media_dev)) {
log_error("media device enumerate failed");
return -1;
}
media_stream_init(&v4l2_media_stream, media_dev);
media_stream_config(&v4l2_media_stream);
log_debug("open media success, fd %d", media_dev->fd);
isp_alg_param_init();
/**
* Unix Socket Operation:
* [Progress] send video device name
*
**/
struct sockaddr_un server_unix, client_unix;
socklen_t client_unix_len;
int size;
if ((listen_fd = socket(AF_UNIX, SOCK_STREAM, 0)) < 0) {
log_error("socket error");
exit(1);
}
memset(&server_unix, 0, sizeof(server_unix));
server_unix.sun_family = AF_UNIX;
strcpy(server_unix.sun_path, server_socket);
size = offsetof(struct sockaddr_un, sun_path) + strlen(server_unix.sun_path);
unlink(server_socket);
if (bind(listen_fd, (struct sockaddr *)&server_unix, size) < 0) {
log_error("bind error");
exit(1);
}
log_debug("UNIX domain socket bound");
if (listen(listen_fd, 16) < 0) {
log_error("listen error");
exit(1);
}
log_debug("Accepting connections ...");
client_unix_len = sizeof(client_unix);
if ((connected_sockfd = accept(listen_fd, (struct sockaddr *)&client_unix, &client_unix_len)) < 0) {
log_error("accept error");
return -1;
}
log_debug("connected_sockfd: %d", connected_sockfd);
char video_dev_name[32] = {0};
strcpy(video_dev_name, v4l2_media_stream.video_ent0->devname);
log_debug("video_dev_name: %s", video_dev_name);
int r = TEMP_FAILURE_RETRY(send(connected_sockfd, video_dev_name, strlen(video_dev_name), 0));
if (r < 0) {
log_error("send video_dev_name, failed");
}
int err;
err = pthread_create(&tparam.process_socket_tid, NULL, &process_socket_thread, &tparam);
if (err != 0) {
log_error("can't create process_socket_thread");
exit(1);
}
err = pthread_create(&tid[MEDIACTRLSRC_STREAM_STATISTICS], NULL, &isp_alg_thread, &tparam);
if (err != 0) {
log_error("can't create isp_alg_thread");
exit(1);
}
pthread_join(tparam.process_socket_tid, NULL);
pthread_join(tid[MEDIACTRLSRC_STREAM_STATISTICS], NULL);
char streamoff_buffer[32] = {0};
strcpy(streamoff_buffer, "streamoff_done");
int m = TEMP_FAILURE_RETRY(send(connected_sockfd, streamoff_buffer, strlen(streamoff_buffer), 0));
if (m < 0) {
log_error("send streamoff_buffer, failed");
}
close(listen_fd);
close(connected_sockfd);
unlink(server_socket);
if (mFlushFd[1] != -1 || mFlushFd[0] != -1) {
close(mFlushFd[0]);
close(mFlushFd[1]);
mFlushFd[0] = -1;
mFlushFd[1] = -1;
}
if (media_dev != NULL) {
log_debug("media reset links, fd %d", media_dev->fd);
media_reset_links(media_dev);
}
log_debug("camctrl exit");
return 0;
}
Reference in New Issue View Git Blame Copy Permalink