282 lines
7.3 KiB
C
282 lines
7.3 KiB
C
// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
|
|
/*
|
|
* Copyright (C) 2025 Amlogic, Inc. All rights reserved
|
|
*/
|
|
|
|
#include <linux/cleanup.h>
|
|
|
|
#include <media/v4l2-ioctl.h>
|
|
#include <media/v4l2-mc.h>
|
|
#include <media/videobuf2-dma-contig.h>
|
|
|
|
#include "c4-isp-common.h"
|
|
#include "hw/c4-isp-hw.h"
|
|
|
|
static void c4_isp_stats_cfg_buff(struct c4_isp_stats *stats)
|
|
{
|
|
stats->buff =
|
|
list_first_entry_or_null(&stats->pending,
|
|
struct c4_isp_stats_buffer, list);
|
|
if (stats->buff) {
|
|
c4_isp_febe_cfg_stats_buff(stats->isp, stats->buff->dma_addr);
|
|
list_del(&stats->buff->list);
|
|
}
|
|
}
|
|
|
|
static int c4_isp_stats_querycap(struct file *file, void *fh,
|
|
struct v4l2_capability *cap)
|
|
{
|
|
strscpy(cap->driver, C4_ISP_DRIVER_NAME, sizeof(cap->driver));
|
|
strscpy(cap->card, "AML C4 ISP5", sizeof(cap->card));
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int c4_isp_stats_enum_fmt(struct file *file, void *fh,
|
|
struct v4l2_fmtdesc *f)
|
|
{
|
|
struct c4_isp_stats *stats = video_drvdata(file);
|
|
|
|
if (f->index > 0 || f->type != stats->vb2_q.type)
|
|
return -EINVAL;
|
|
|
|
f->pixelformat = V4L2_META_FMT_C4ISP_STATS;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int c4_isp_stats_g_fmt(struct file *file, void *fh,
|
|
struct v4l2_format *f)
|
|
{
|
|
struct c4_isp_stats *stats = video_drvdata(file);
|
|
|
|
f->fmt.meta = stats->vfmt.fmt.meta;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static const struct v4l2_ioctl_ops isp_stats_v4l2_ioctl_ops = {
|
|
.vidioc_querycap = c4_isp_stats_querycap,
|
|
.vidioc_enum_fmt_meta_cap = c4_isp_stats_enum_fmt,
|
|
.vidioc_g_fmt_meta_cap = c4_isp_stats_g_fmt,
|
|
.vidioc_s_fmt_meta_cap = c4_isp_stats_g_fmt,
|
|
.vidioc_try_fmt_meta_cap = c4_isp_stats_g_fmt,
|
|
.vidioc_reqbufs = vb2_ioctl_reqbufs,
|
|
.vidioc_querybuf = vb2_ioctl_querybuf,
|
|
.vidioc_qbuf = vb2_ioctl_qbuf,
|
|
.vidioc_expbuf = vb2_ioctl_expbuf,
|
|
.vidioc_dqbuf = vb2_ioctl_dqbuf,
|
|
.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
|
|
.vidioc_create_bufs = vb2_ioctl_create_bufs,
|
|
.vidioc_streamon = vb2_ioctl_streamon,
|
|
.vidioc_streamoff = vb2_ioctl_streamoff,
|
|
};
|
|
|
|
static const struct v4l2_file_operations isp_stats_v4l2_fops = {
|
|
.open = v4l2_fh_open,
|
|
.release = vb2_fop_release,
|
|
.poll = vb2_fop_poll,
|
|
.unlocked_ioctl = video_ioctl2,
|
|
.mmap = vb2_fop_mmap,
|
|
};
|
|
|
|
static int c4_isp_stats_vb2_queue_setup(struct vb2_queue *q,
|
|
unsigned int *num_buffers,
|
|
unsigned int *num_planes,
|
|
unsigned int sizes[],
|
|
struct device *alloc_devs[])
|
|
{
|
|
if (*num_planes) {
|
|
if (*num_planes != 1)
|
|
return -EINVAL;
|
|
|
|
if (sizes[0] < sizeof(struct c4_isp_stats_info))
|
|
return -EINVAL;
|
|
|
|
return 0;
|
|
}
|
|
|
|
*num_planes = 1;
|
|
sizes[0] = sizeof(struct c4_isp_stats_info);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static void c4_isp_stats_vb2_buf_queue(struct vb2_buffer *vb)
|
|
{
|
|
struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
|
|
struct c4_isp_stats_buffer *buf =
|
|
container_of(v4l2_buf, struct c4_isp_stats_buffer, vb);
|
|
struct c4_isp_stats *stats = vb2_get_drv_priv(vb->vb2_queue);
|
|
|
|
guard(spinlock_irqsave)(&stats->buff_lock);
|
|
|
|
list_add_tail(&buf->list, &stats->pending);
|
|
}
|
|
|
|
static int c4_isp_stats_vb2_buf_prepare(struct vb2_buffer *vb)
|
|
{
|
|
struct c4_isp_stats *stats = vb2_get_drv_priv(vb->vb2_queue);
|
|
unsigned int size = stats->vfmt.fmt.meta.buffersize;
|
|
|
|
if (vb2_plane_size(vb, 0) < size) {
|
|
dev_err(stats->isp->dev,
|
|
"User buffer too small (%ld < %u)\n",
|
|
vb2_plane_size(vb, 0), size);
|
|
return -EINVAL;
|
|
}
|
|
|
|
vb2_set_plane_payload(vb, 0, size);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int c4_isp_stats_vb2_buf_init(struct vb2_buffer *vb)
|
|
{
|
|
struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
|
|
struct c4_isp_stats_buffer *buf =
|
|
container_of(v4l2_buf, struct c4_isp_stats_buffer, vb);
|
|
|
|
buf->dma_addr = vb2_dma_contig_plane_dma_addr(vb, 0);
|
|
buf->vaddr = vb2_plane_vaddr(vb, 0);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static void c4_isp_stats_vb2_stop_streaming(struct vb2_queue *q)
|
|
{
|
|
struct c4_isp_stats *stats = vb2_get_drv_priv(q);
|
|
|
|
guard(spinlock_irqsave)(&stats->buff_lock);
|
|
|
|
if (stats->buff) {
|
|
vb2_buffer_done(&stats->buff->vb.vb2_buf, VB2_BUF_STATE_ERROR);
|
|
stats->buff = NULL;
|
|
}
|
|
|
|
while (!list_empty(&stats->pending)) {
|
|
struct c4_isp_stats_buffer *buff;
|
|
|
|
buff = list_first_entry(&stats->pending,
|
|
struct c4_isp_stats_buffer, list);
|
|
list_del(&buff->list);
|
|
vb2_buffer_done(&buff->vb.vb2_buf, VB2_BUF_STATE_ERROR);
|
|
}
|
|
}
|
|
|
|
static const struct vb2_ops isp_stats_vb2_ops = {
|
|
.queue_setup = c4_isp_stats_vb2_queue_setup,
|
|
.buf_queue = c4_isp_stats_vb2_buf_queue,
|
|
.buf_prepare = c4_isp_stats_vb2_buf_prepare,
|
|
.buf_init = c4_isp_stats_vb2_buf_init,
|
|
.wait_prepare = vb2_ops_wait_prepare,
|
|
.wait_finish = vb2_ops_wait_finish,
|
|
.stop_streaming = c4_isp_stats_vb2_stop_streaming,
|
|
};
|
|
|
|
int c4_isp_stats_register(struct c4_isp_device *isp)
|
|
{
|
|
struct c4_isp_stats *stats = &isp->stats;
|
|
struct video_device *vdev = &stats->vdev;
|
|
struct vb2_queue *vb2_q = &stats->vb2_q;
|
|
int ret;
|
|
|
|
memset(stats, 0, sizeof(*stats));
|
|
stats->vfmt.fmt.meta.dataformat = V4L2_META_FMT_C4ISP_STATS;
|
|
stats->vfmt.fmt.meta.buffersize = sizeof(struct c4_isp_stats_info);
|
|
stats->isp = isp;
|
|
INIT_LIST_HEAD(&stats->pending);
|
|
spin_lock_init(&stats->buff_lock);
|
|
|
|
mutex_init(&stats->lock);
|
|
|
|
snprintf(vdev->name, sizeof(vdev->name), "c4-isp-stats");
|
|
vdev->fops = &isp_stats_v4l2_fops;
|
|
vdev->ioctl_ops = &isp_stats_v4l2_ioctl_ops;
|
|
vdev->v4l2_dev = &isp->v4l2_dev;
|
|
vdev->lock = &stats->lock;
|
|
vdev->minor = -1;
|
|
vdev->queue = vb2_q;
|
|
vdev->release = video_device_release_empty;
|
|
vdev->device_caps = V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING;
|
|
vdev->vfl_dir = VFL_DIR_RX;
|
|
video_set_drvdata(vdev, stats);
|
|
|
|
vb2_q->drv_priv = stats;
|
|
vb2_q->mem_ops = &vb2_dma_contig_memops;
|
|
vb2_q->ops = &isp_stats_vb2_ops;
|
|
vb2_q->type = V4L2_BUF_TYPE_META_CAPTURE;
|
|
vb2_q->io_modes = VB2_DMABUF | VB2_MMAP;
|
|
vb2_q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
|
|
vb2_q->buf_struct_size = sizeof(struct c4_isp_stats_buffer);
|
|
vb2_q->dev = isp->dev;
|
|
vb2_q->lock = &stats->lock;
|
|
vb2_q->min_queued_buffers = 2;
|
|
|
|
ret = vb2_queue_init(vb2_q);
|
|
if (ret)
|
|
goto err_destroy;
|
|
|
|
stats->pad.flags = MEDIA_PAD_FL_SINK;
|
|
ret = media_entity_pads_init(&vdev->entity, 1, &stats->pad);
|
|
if (ret)
|
|
goto err_queue_release;
|
|
|
|
ret = video_register_device(vdev, VFL_TYPE_VIDEO, C4_V4L2_VIDEO_BEGIN_NUM);
|
|
if (ret) {
|
|
dev_err(isp->dev,
|
|
"Failed to register %s: %d\n", vdev->name, ret);
|
|
goto err_entity_cleanup;
|
|
}
|
|
|
|
return 0;
|
|
|
|
err_entity_cleanup:
|
|
media_entity_cleanup(&vdev->entity);
|
|
err_queue_release:
|
|
vb2_queue_release(vb2_q);
|
|
err_destroy:
|
|
mutex_destroy(&stats->lock);
|
|
return ret;
|
|
}
|
|
|
|
void c4_isp_stats_unregister(struct c4_isp_device *isp)
|
|
{
|
|
struct c4_isp_stats *stats = &isp->stats;
|
|
|
|
vb2_queue_release(&stats->vb2_q);
|
|
media_entity_cleanup(&stats->vdev.entity);
|
|
video_unregister_device(&stats->vdev);
|
|
mutex_destroy(&stats->lock);
|
|
}
|
|
|
|
void c4_isp_stats_pre_cfg(struct c4_isp_device *isp)
|
|
{
|
|
struct c4_isp_stats *stats = &isp->stats;
|
|
|
|
guard(spinlock_irqsave)(&stats->buff_lock);
|
|
|
|
c4_isp_stats_cfg_buff(stats);
|
|
}
|
|
|
|
void c4_isp_stats_isr(struct c4_isp_device *isp)
|
|
{
|
|
struct c4_isp_stats *stats = &isp->stats;
|
|
|
|
if (!stats->vb2_q.streaming)
|
|
return;
|
|
|
|
guard(spinlock_irqsave)(&stats->buff_lock);
|
|
|
|
if (stats->buff) {
|
|
c4_isp_febe_get_stats_info(isp, stats->buff->vaddr);
|
|
|
|
stats->buff->vb.sequence = stats->isp->be_frm_seq;
|
|
stats->buff->vb.vb2_buf.timestamp = ktime_get();
|
|
stats->buff->vb.field = V4L2_FIELD_NONE;
|
|
vb2_buffer_done(&stats->buff->vb.vb2_buf, VB2_BUF_STATE_DONE);
|
|
}
|
|
|
|
c4_isp_stats_cfg_buff(stats);
|
|
}
|