BPI-CM9-Linux/aml-comp/hardware/amlogic/arm-isp-6.12/aml_isp5/driver/isp5/c4-isp-params.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-vmalloc.h>

#include "c4-isp-common.h"
#include "hw/c4-isp-config.h"
#include "hw/c4-isp-hw.h"

typedef void (*c4_isp_block_handler)(struct c4_isp_device *isp,
				     const union c4_isp_params_block *block);

struct c4_isp_params_handler {
	size_t size;
	c4_isp_block_handler handler;
};

#define to_c4_isp_params_buffer(vbuf) \
	container_of(vbuf, struct c4_isp_params_buffer, vb)

static const struct c4_isp_params_handler c4_isp_params_handlers[] = {
	[C4_ISP_PARAMS_BLOCK_AWB_GAINS] = {
		.size = sizeof(struct c4_isp_params_awb_gains),
		.handler = c4_isp_febe_cfg_awb_gains,
	},
	[C4_ISP_PARAMS_BLOCK_AWB_CONFIG] = {
		.size = sizeof(struct c4_isp_params_awb_config),
		.handler = c4_isp_febe_cfg_awb_config,
	},
	[C4_ISP_PARAMS_BLOCK_AE_CONFIG] = {
		.size = sizeof(struct c4_isp_params_ae_config),
		.handler = c4_isp_febe_cfg_ae_config,
	},
	[C4_ISP_PARAMS_BLOCK_AF_CONFIG] = {
		.size = sizeof(struct c4_isp_params_af_config),
		.handler = c4_isp_febe_cfg_af_config,
	},
	[C4_ISP_PARAMS_BLOCK_FLKR_CONFIG] = {
		.size = sizeof(struct c4_isp_params_flkr_config),
		.handler = c4_isp_febe_cfg_flkr_config,
	},
	[C4_ISP_PARAMS_BLOCK_POST_CONFIG] = {
		.size = sizeof(struct c4_isp_params_post_config),
		.handler = c4_isp_febe_cfg_post_config,
	},
	[C4_ISP_PARAMS_BLOCK_PST_GAMMA] = {
		.size = sizeof(struct c4_isp_params_pst_gamma),
		.handler = c4_isp_febe_cfg_pst_gamma,
	},
	[C4_ISP_PARAMS_BLOCK_CCM] = {
		.size = sizeof(struct c4_isp_params_ccm),
		.handler = c4_isp_febe_cfg_ccm,
	},
	[C4_ISP_PARAMS_BLOCK_CSC] = {
		.size = sizeof(struct c4_isp_params_csc),
		.handler = c4_isp_febe_cfg_csc,
	},
	[C4_ISP_PARAMS_BLOCK_BLC] = {
		.size = sizeof(struct c4_isp_params_blc),
		.handler = c4_isp_febe_cfg_blc,
	},
	[C4_ISP_PARAMS_BLOCK_GTM] = {
		.size = sizeof(struct c4_isp_params_gtm),
		.handler = c4_isp_febe_cfg_gtm,
	},
	[C4_ISP_PARAMS_BLOCK_LENS_MESH] = {
		.size = sizeof(struct c4_isp_params_lens_mesh),
		.handler = c4_isp_febe_cfg_lens_mesh,
	},
	[C4_ISP_PARAMS_BLOCK_LENS_RAD] = {
		.size = sizeof(struct c4_isp_params_lens_rad),
		.handler = c4_isp_febe_cfg_lens_rad,
	},
	[C4_ISP_PARAMS_BLOCK_LENS_EOTF1] = {
		.size = sizeof(struct c4_isp_params_lens_eotf1),
		.handler = c4_isp_febe_cfg_lens_eotf1,
	},
	[C4_ISP_PARAMS_BLOCK_SQRT] = {
		.size = sizeof(struct c4_isp_params_sqrt),
		.handler = c4_isp_febe_cfg_sqrt,
	},
	[C4_ISP_PARAMS_BLOCK_LUT3D] = {
		.size = sizeof(struct c4_isp_params_lut3d),
		.handler = c4_isp_febe_cfg_lut3d,
	},
	[C4_ISP_PARAMS_BLOCK_DPC] = {
		.size = sizeof(struct c4_isp_params_dpc),
		.handler = c4_isp_febe_cfg_dpc,
	},
	[C4_ISP_PARAMS_BLOCK_YUVTNR] = {
		.size = sizeof(struct c4_isp_params_yuvtnr),
		.handler = c4_isp_febe_cfg_yuvtnr,
	},
	[C4_ISP_PARAMS_BLOCK_YUVSNR] = {
		.size = sizeof(struct c4_isp_params_yuvsnr),
		.handler = c4_isp_febe_cfg_yuvsnr,
	},
	[C4_ISP_PARAMS_BLOCK_YUVCNR] = {
		.size = sizeof(struct c4_isp_params_yuvcnr),
		.handler = c4_isp_febe_cfg_yuvcnr,
	},
	[C4_ISP_PARAMS_BLOCK_CUBIC] = {
		.size = sizeof(struct c4_isp_params_cubic),
		.handler = c4_isp_febe_cfg_cubic,
	},
	[C4_ISP_PARAMS_BLOCK_DMSPP] = {
		.size = sizeof(struct c4_isp_params_dmspp),
		.handler = c4_isp_febe_cfg_dmspp,
	},
	[C4_ISP_PARAMS_BLOCK_GE] = {
		.size = sizeof(struct c4_isp_params_ge),
		.handler = c4_isp_febe_cfg_ge,
	},
	[C4_ISP_PARAMS_BLOCK_RAWSNR] = {
		.size = sizeof(struct c4_isp_params_rawsnr),
		.handler = c4_isp_febe_cfg_rawsnr,
	},
	[C4_ISP_PARAMS_BLOCK_RAWTNR] = {
		.size = sizeof(struct c4_isp_params_rawtnr),
		.handler = c4_isp_febe_cfg_rawtnr,
	},
	[C4_ISP_PARAMS_BLOCK_DMS] = {
		.size = sizeof(struct c4_isp_params_dms),
		.handler = c4_isp_febe_cfg_dms,
	},
	[C4_ISP_PARAMS_BLOCK_DGAIN] = {
		.size = sizeof(struct c4_isp_params_dgain),
		.handler = c4_isp_febe_cfg_dgain,
	},
	[C4_ISP_PARAMS_BLOCK_IDGAIN] = {
		.size = sizeof(struct c4_isp_params_idgain),
		.handler = c4_isp_febe_cfg_idgain,
	},
	[C4_ISP_PARAMS_BLOCK_PK] = {
		.size = sizeof(struct c4_isp_params_pk),
		.handler = c4_isp_febe_cfg_pk,
	},
	[C4_ISP_PARAMS_BLOCK_AMCM] = {
		.size = sizeof(struct c4_isp_params_amcm),
		.handler = c4_isp_febe_cfg_amcm,
	},
	[C4_ISP_PARAMS_BLOCK_WDR] = {
		.size = sizeof(struct c4_isp_params_wdr),
		.handler = c4_isp_febe_cfg_wdr,
	},
	[C4_ISP_PARAMS_BLOCK_LTM] = {
		.size = sizeof(struct c4_isp_params_ltm),
		.handler = c4_isp_febe_cfg_ltm,
	},
	[C4_ISP_PARAMS_BLOCK_LC] = {
		.size = sizeof(struct c4_isp_params_lc),
		.handler = c4_isp_febe_cfg_lc,
	},
	[C4_ISP_PARAMS_BLOCK_DNLP] = {
		.size = sizeof(struct c4_isp_params_dnlp),
		.handler = c4_isp_febe_cfg_dnlp,
	},
	[C4_ISP_PARAMS_BLOCK_DHZ] = {
		.size = sizeof(struct c4_isp_params_dhz),
		.handler = c4_isp_febe_cfg_dhz,
	},
	[C4_ISP_PARAMS_BLOCK_CAC] = {
		.size = sizeof(struct c4_isp_params_cac),
		.handler = c4_isp_febe_cfg_cac,
	},
	[C4_ISP_PARAMS_BLOCK_LBLC] = {
		.size = sizeof(struct c4_isp_params_lblc),
		.handler = c4_isp_febe_cfg_lblc,
	},
	[C4_ISP_PARAMS_BLOCK_FE_BLC] = {
		.size = sizeof(struct c4_isp_params_fe_blc),
		.handler = c4_isp_febe_cfg_fe_blc,
	},
	[C4_ISP_PARAMS_BLOCK_BAC] = {
		.size = sizeof(struct c4_isp_params_bac),
		.handler = c4_isp_febe_cfg_bac,
	},
	[C4_ISP_PARAMS_BLOCK_BITCMB] = {
		.size = sizeof(struct c4_isp_params_bitcmb),
		.handler = c4_isp_febe_cfg_bitcmb,
	},
	[C4_ISP_PARAMS_BLOCK_AFC] = {
		.size = sizeof(struct c4_isp_params_afc_ctl),
		.handler = c4_isp_febe_cfg_afc,
	},
	[C4_ISP_PARAMS_BLOCK_DBG] = {
		.size = sizeof(struct c4_isp_params_dbg),
		.handler = c4_isp_febe_cfg_dbg,
	},
};

static void c4_isp_params_cfg_blocks(struct c4_isp_params *params)
{
	struct c4_isp_params_cfg *config = params->buff->cfg;
	size_t block_offset = 0;

	if (WARN_ON(!config))
		return;

	/* Walk the list of parameter blocks and process them */
	while (block_offset < config->data_size) {
		const struct c4_isp_params_handler *block_handler;
		const union c4_isp_params_block *block;

		block = (const union c4_isp_params_block *)
			 &config->data[block_offset];

		block_handler = &c4_isp_params_handlers[block->header.type];
		block_handler->handler(params->isp, block);

		block_offset += block->header.size;
	}
}

/* V4L2 video operations */

static int c4_isp_params_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_params_enum_fmt(struct file *file, void *fh,
				  struct v4l2_fmtdesc *f)
{
	if (f->index)
		return -EINVAL;

	f->pixelformat = V4L2_META_FMT_C4ISP_PARAMS;

	return 0;
}

static int c4_isp_params_g_fmt(struct file *file, void *fh,
			       struct v4l2_format *f)
{
	struct c4_isp_params *params = video_drvdata(file);

	f->fmt.meta = params->vfmt.fmt.meta;

	return 0;
}

static const struct v4l2_ioctl_ops isp_params_v4l2_ioctl_ops = {
	.vidioc_querycap                = c4_isp_params_querycap,
	.vidioc_enum_fmt_meta_out       = c4_isp_params_enum_fmt,
	.vidioc_g_fmt_meta_out          = c4_isp_params_g_fmt,
	.vidioc_s_fmt_meta_out          = c4_isp_params_g_fmt,
	.vidioc_try_fmt_meta_out        = c4_isp_params_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_params_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_params_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_params_cfg))
			return -EINVAL;

		return 0;
	}

	*num_planes = 1;
	sizes[0] = sizeof(struct c4_isp_params_cfg);

	return 0;
}

static void c4_isp_params_vb2_buf_queue(struct vb2_buffer *vb)
{
	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
	struct c4_isp_params_buffer *buf = to_c4_isp_params_buffer(v4l2_buf);
	struct c4_isp_params *params = vb2_get_drv_priv(vb->vb2_queue);

	guard(spinlock_irqsave)(&params->buff_lock);

	list_add_tail(&buf->list, &params->pending);
}

static int c4_isp_params_vb2_buf_prepare(struct vb2_buffer *vb)
{
	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
	struct c4_isp_params_buffer *buf = to_c4_isp_params_buffer(vbuf);
	struct c4_isp_params *params = vb2_get_drv_priv(vb->vb2_queue);
	struct c4_isp_params_cfg *cfg = buf->cfg;
	struct c4_isp_params_cfg *usr_cfg = vb2_plane_vaddr(vb, 0);
	size_t payload_size = vb2_get_plane_payload(vb, 0);
	size_t header_size = offsetof(struct c4_isp_params_cfg, data);
	size_t block_offset = 0;
	size_t cfg_size;

	/* Payload size can't be greater than the destination buffer size */
	if (payload_size > params->vfmt.fmt.meta.buffersize) {
		dev_err(params->isp->dev,
			"Payload size is too large: %zu\n", payload_size);
		return -EINVAL;
	}

	/* Payload size can't be smaller than the header size */
	if (payload_size < header_size) {
		dev_err(params->isp->dev,
			"Payload size is too small: %zu\n", payload_size);
		return -EINVAL;
	}

	/*
	 * Use the internal scratch buffer to avoid userspace modifying
	 * the buffer content while the driver is processing it.
	 */
	memcpy(cfg, usr_cfg, payload_size);

	/* Only v0 is supported at the moment */
	if (cfg->version != C4_ISP_PARAMS_BUFFER_V0) {
		dev_err(params->isp->dev,
			"Invalid params buffer version: %u\n", cfg->version);
		return -EINVAL;
	}

	/* Validate the size reported in the parameter buffer header */
	cfg_size = header_size + cfg->data_size;
	if (cfg_size != payload_size) {
		dev_err(params->isp->dev,
			"Data size %zu and payload size %zu are different\n",
			cfg_size, payload_size);
		return -EINVAL;
	}

	/* Walk the list of parameter blocks and validate them */
	cfg_size = cfg->data_size;
	while (cfg_size >= sizeof(struct c4_isp_params_block_header)) {
		const struct c4_isp_params_block_header *block;
		const struct c4_isp_params_handler *handler;

		block = (struct c4_isp_params_block_header *)
			&cfg->data[block_offset];

		if (block->type >= ARRAY_SIZE(c4_isp_params_handlers)) {
			dev_err(params->isp->dev,
				"Invalid params block type\n");
			return -EINVAL;
		}

		if (block->size > cfg_size) {
			dev_err(params->isp->dev,
				"Block size is greater than cfg size\n");
			return -EINVAL;
		}

		if ((block->flags & (C4_ISP_PARAMS_BLOCK_FL_ENABLE |
				     C4_ISP_PARAMS_BLOCK_FL_DISABLE)) ==
		    (C4_ISP_PARAMS_BLOCK_FL_ENABLE |
		     C4_ISP_PARAMS_BLOCK_FL_DISABLE)) {
			dev_err(params->isp->dev,
				"Invalid parameters block flags\n");
			return -EINVAL;
		}

		handler = &c4_isp_params_handlers[block->type];
		if (block->size != handler->size) {
			dev_err(params->isp->dev,
				"Invalid params block size\n");
			return -EINVAL;
		}

		block_offset += block->size;
		cfg_size -= block->size;
	}

	if (cfg_size) {
		dev_err(params->isp->dev,
			"Unexpected data after the params buffer end\n");
		return -EINVAL;
	}

	return 0;
}

static int c4_isp_params_vb2_buf_init(struct vb2_buffer *vb)
{
	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
	struct c4_isp_params *params = vb2_get_drv_priv(vb->vb2_queue);
	struct c4_isp_params_buffer *buf = to_c4_isp_params_buffer(v4l2_buf);

	buf->cfg = kvmalloc(params->vfmt.fmt.meta.buffersize, GFP_KERNEL);
	if (!buf->cfg)
		return -ENOMEM;

	return 0;
}

static void c4_isp_params_vb2_buf_cleanup(struct vb2_buffer *vb)
{
	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
	struct c4_isp_params_buffer *buf = to_c4_isp_params_buffer(v4l2_buf);

	kvfree(buf->cfg);
	buf->cfg = NULL;
}

static void c4_isp_params_vb2_stop_streaming(struct vb2_queue *q)
{
	struct c4_isp_params *params = vb2_get_drv_priv(q);
	struct c4_isp_params_buffer *buff;

	guard(spinlock_irqsave)(&params->buff_lock);

	while (!list_empty(&params->pending)) {
		buff = list_first_entry(&params->pending,
					struct c4_isp_params_buffer, list);
		list_del(&buff->list);
		vb2_buffer_done(&buff->vb.vb2_buf, VB2_BUF_STATE_ERROR);
	}
}

static const struct vb2_ops isp_params_vb2_ops = {
	.queue_setup = c4_isp_params_vb2_queue_setup,
	.buf_queue = c4_isp_params_vb2_buf_queue,
	.buf_prepare = c4_isp_params_vb2_buf_prepare,
	.buf_init = c4_isp_params_vb2_buf_init,
	.buf_cleanup = c4_isp_params_vb2_buf_cleanup,
	.wait_prepare = vb2_ops_wait_prepare,
	.wait_finish = vb2_ops_wait_finish,
	.stop_streaming = c4_isp_params_vb2_stop_streaming,
};

int c4_isp_params_register(struct c4_isp_device *isp)
{
	struct c4_isp_params *params = &isp->params;
	struct video_device *vdev = &params->vdev;
	struct vb2_queue *vb2_q = &params->vb2_q;
	int ret;

	memset(params, 0, sizeof(*params));
	params->vfmt.fmt.meta.dataformat = V4L2_META_FMT_C4ISP_PARAMS;
	params->vfmt.fmt.meta.buffersize = sizeof(struct c4_isp_params_cfg);
	params->isp = isp;
	INIT_LIST_HEAD(&params->pending);
	spin_lock_init(&params->buff_lock);
	mutex_init(&params->lock);

	snprintf(vdev->name, sizeof(vdev->name), "c4-isp-params");
	vdev->fops = &isp_params_v4l2_fops;
	vdev->ioctl_ops = &isp_params_v4l2_ioctl_ops;
	vdev->v4l2_dev = &isp->v4l2_dev;
	vdev->lock = &params->lock;
	vdev->minor = -1;
	vdev->queue = vb2_q;
	vdev->release = video_device_release_empty;
	vdev->device_caps = V4L2_CAP_META_OUTPUT | V4L2_CAP_STREAMING;
	vdev->vfl_dir = VFL_DIR_TX;
	video_set_drvdata(vdev, params);

	vb2_q->drv_priv = params;
	vb2_q->mem_ops = &vb2_vmalloc_memops;
	vb2_q->ops = &isp_params_vb2_ops;
	vb2_q->type = V4L2_BUF_TYPE_META_OUTPUT;
	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_params_buffer);
	vb2_q->dev = isp->dev;
	vb2_q->lock = &params->lock;
	vb2_q->min_queued_buffers = 1;

	ret = vb2_queue_init(vb2_q);
	if (ret)
		goto err_detroy;

	params->pad.flags = MEDIA_PAD_FL_SOURCE;
	ret = media_entity_pads_init(&vdev->entity, 1, &params->pad);
	if (ret)
		goto err_queue_release;

	ret = video_register_device(vdev, VFL_TYPE_VIDEO, C4_V4L2_VIDEO_BEGIN_NUM);
	if (ret < 0) {
		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_detroy:
	mutex_destroy(&params->lock);
	return ret;
}

void c4_isp_params_unregister(struct c4_isp_device *isp)
{
	struct c4_isp_params *params = &isp->params;

	vb2_queue_release(&params->vb2_q);
	media_entity_cleanup(&params->vdev.entity);
	video_unregister_device(&params->vdev);
	mutex_destroy(&params->lock);
}

void c4_isp_params_pre_cfg(struct c4_isp_device *isp)
{
	struct c4_isp_params *params = &isp->params;

	guard(spinlock_irqsave)(&params->buff_lock);

	/* Only use the first buffer to initialize ISP */
	params->buff =
		list_first_entry_or_null(&params->pending,
					 struct c4_isp_params_buffer, list);
	if (params->buff)
		c4_isp_params_cfg_blocks(params);
}

void c4_isp_params_isr(struct c4_isp_device *isp)
{
	struct c4_isp_params *params = &isp->params;

	if (!params->vb2_q.streaming)
		return;

	guard(spinlock_irqsave)(&params->buff_lock);

	params->buff =
		list_first_entry_or_null(&params->pending,
					 struct c4_isp_params_buffer, list);
	if (!params->buff)
		return;

	list_del(&params->buff->list);

	c4_isp_params_cfg_blocks(params);

	params->buff->vb.sequence = params->isp->be_frm_seq;
	params->buff->vb.vb2_buf.timestamp = ktime_get();
	params->buff->vb.field = V4L2_FIELD_NONE;
	vb2_buffer_done(&params->buff->vb.vb2_buf, VB2_BUF_STATE_DONE);
}