mali-c55-stats.c

  1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * ARM Mali-C55 ISP Driver - 3A Statistics capture device
  4 *
  5 * Copyright (C) 2025 Ideas on Board Oy
  6 */
  7
  8#include <linux/container_of.h>
  9#include <linux/dev_printk.h>
 10#include <linux/list.h>
 11#include <linux/media/arm/mali-c55-config.h>
 12#include <linux/mutex.h>
 13#include <linux/pm_runtime.h>
 14#include <linux/spinlock.h>
 15#include <linux/string.h>
 16
 17#include <media/media-entity.h>
 18#include <media/v4l2-dev.h>
 19#include <media/v4l2-event.h>
 20#include <media/v4l2-fh.h>
 21#include <media/v4l2-ioctl.h>
 22#include <media/videobuf2-core.h>
 23#include <media/videobuf2-dma-contig.h>
 24
 25#include "mali-c55-common.h"
 26#include "mali-c55-registers.h"
 27
 28static const unsigned int metering_space_addrs[] = {
 29	[MALI_C55_CONFIG_PING] = 0x095ac,
 30	[MALI_C55_CONFIG_PONG] = 0x2156c,
 31};
 32
 33static int mali_c55_stats_enum_fmt_meta_cap(struct file *file, void *fh,
 34					    struct v4l2_fmtdesc *f)
 35{
 36	if (f->index)
 37		return -EINVAL;
 38
 39	f->pixelformat = V4L2_META_FMT_MALI_C55_STATS;
 40
 41	return 0;
 42}
 43
 44static int mali_c55_stats_g_fmt_meta_cap(struct file *file, void *fh,
 45					 struct v4l2_format *f)
 46{
 47	static const struct v4l2_meta_format mfmt = {
 48		.dataformat = V4L2_META_FMT_MALI_C55_STATS,
 49		.buffersize = sizeof(struct mali_c55_stats_buffer)
 50	};
 51
 52	f->fmt.meta = mfmt;
 53
 54	return 0;
 55}
 56
 57static int mali_c55_stats_querycap(struct file *file,
 58				   void *priv, struct v4l2_capability *cap)
 59{
 60	strscpy(cap->driver, MALI_C55_DRIVER_NAME, sizeof(cap->driver));
 61	strscpy(cap->card, "ARM Mali-C55 ISP", sizeof(cap->card));
 62
 63	return 0;
 64}
 65
 66static const struct v4l2_ioctl_ops mali_c55_stats_v4l2_ioctl_ops = {
 67	.vidioc_reqbufs = vb2_ioctl_reqbufs,
 68	.vidioc_querybuf = vb2_ioctl_querybuf,
 69	.vidioc_create_bufs = vb2_ioctl_create_bufs,
 70	.vidioc_qbuf = vb2_ioctl_qbuf,
 71	.vidioc_expbuf = vb2_ioctl_expbuf,
 72	.vidioc_dqbuf = vb2_ioctl_dqbuf,
 73	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
 74	.vidioc_streamon = vb2_ioctl_streamon,
 75	.vidioc_streamoff = vb2_ioctl_streamoff,
 76	.vidioc_enum_fmt_meta_cap = mali_c55_stats_enum_fmt_meta_cap,
 77	.vidioc_g_fmt_meta_cap = mali_c55_stats_g_fmt_meta_cap,
 78	.vidioc_s_fmt_meta_cap = mali_c55_stats_g_fmt_meta_cap,
 79	.vidioc_try_fmt_meta_cap = mali_c55_stats_g_fmt_meta_cap,
 80	.vidioc_querycap = mali_c55_stats_querycap,
 81	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 82	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 83};
 84
 85static const struct v4l2_file_operations mali_c55_stats_v4l2_fops = {
 86	.owner = THIS_MODULE,
 87	.unlocked_ioctl = video_ioctl2,
 88	.open = v4l2_fh_open,
 89	.release = vb2_fop_release,
 90	.poll = vb2_fop_poll,
 91	.mmap = vb2_fop_mmap,
 92};
 93
 94static int
 95mali_c55_stats_queue_setup(struct vb2_queue *q, unsigned int *num_buffers,
 96			   unsigned int *num_planes, unsigned int sizes[],
 97			   struct device *alloc_devs[])
 98{
 99	if (*num_planes && *num_planes > 1)
100		return -EINVAL;
101
102	if (sizes[0] && sizes[0] < sizeof(struct mali_c55_stats_buffer))
103		return -EINVAL;
104
105	*num_planes = 1;
106
107	if (!sizes[0])
108		sizes[0] = sizeof(struct mali_c55_stats_buffer);
109
110	return 0;
111}
112
113static void mali_c55_stats_buf_queue(struct vb2_buffer *vb)
114{
115	struct mali_c55_stats *stats = vb2_get_drv_priv(vb->vb2_queue);
116	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
117	struct mali_c55_stats_buf *buf = container_of(vbuf,
118						struct mali_c55_stats_buf, vb);
119
120	vb2_set_plane_payload(vb, 0, sizeof(struct mali_c55_stats_buffer));
121	buf->segments_remaining = 2;
122	buf->failed = false;
123
124	spin_lock(&stats->buffers.lock);
125	list_add_tail(&buf->queue, &stats->buffers.queue);
126	spin_unlock(&stats->buffers.lock);
127}
128
129static void mali_c55_stats_return_buffers(struct mali_c55_stats *stats,
130					  enum vb2_buffer_state state)
131{
132	struct mali_c55_stats_buf *buf, *tmp;
133
134	guard(spinlock)(&stats->buffers.lock);
135
136	list_for_each_entry_safe(buf, tmp, &stats->buffers.queue, queue) {
137		list_del(&buf->queue);
138		vb2_buffer_done(&buf->vb.vb2_buf, state);
139	}
140}
141
142static int mali_c55_stats_start_streaming(struct vb2_queue *q,
143					  unsigned int count)
144{
145	struct mali_c55_stats *stats = vb2_get_drv_priv(q);
146	struct mali_c55 *mali_c55 = stats->mali_c55;
147	int ret;
148
149	ret = pm_runtime_resume_and_get(mali_c55->dev);
150	if (ret)
151		goto err_return_buffers;
152
153	ret = video_device_pipeline_alloc_start(&stats->vdev);
154	if (ret)
155		goto err_pm_put;
156
157	if (mali_c55_pipeline_ready(mali_c55)) {
158		ret = v4l2_subdev_enable_streams(&mali_c55->isp.sd,
159						 MALI_C55_ISP_PAD_SOURCE_VIDEO,
160						 BIT(0));
161		if (ret < 0)
162			goto err_stop_pipeline;
163	}
164
165	return 0;
166
167err_stop_pipeline:
168	video_device_pipeline_stop(&stats->vdev);
169err_pm_put:
170	pm_runtime_put_autosuspend(mali_c55->dev);
171err_return_buffers:
172	mali_c55_stats_return_buffers(stats, VB2_BUF_STATE_QUEUED);
173
174	return ret;
175}
176
177static void mali_c55_stats_stop_streaming(struct vb2_queue *q)
178{
179	struct mali_c55_stats *stats = vb2_get_drv_priv(q);
180	struct mali_c55 *mali_c55 = stats->mali_c55;
181	struct mali_c55_isp *isp = &mali_c55->isp;
182
183	if (mali_c55_pipeline_ready(mali_c55)) {
184		if (v4l2_subdev_is_streaming(&isp->sd))
185			v4l2_subdev_disable_streams(&isp->sd,
186						MALI_C55_ISP_PAD_SOURCE_VIDEO,
187						BIT(0));
188	}
189
190	video_device_pipeline_stop(&stats->vdev);
191	mali_c55_stats_return_buffers(stats, VB2_BUF_STATE_ERROR);
192
193	pm_runtime_put_autosuspend(stats->mali_c55->dev);
194}
195
196static const struct vb2_ops mali_c55_stats_vb2_ops = {
197	.queue_setup = mali_c55_stats_queue_setup,
198	.buf_queue = mali_c55_stats_buf_queue,
199	.start_streaming = mali_c55_stats_start_streaming,
200	.stop_streaming = mali_c55_stats_stop_streaming,
201};
202
203static void mali_c55_stats_cpu_read(struct mali_c55_stats *stats,
204				    struct mali_c55_stats_buf *buf,
205				    enum mali_c55_config_spaces cfg_space)
206{
207	struct mali_c55 *mali_c55 = stats->mali_c55;
208	const void __iomem *src;
209	size_t length;
210	void *dst;
211
212	src = mali_c55->base + MALI_C55_REG_1024BIN_HIST;
213	dst = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
214	memcpy_fromio(dst, src, MALI_C55_1024BIN_HIST_SIZE);
215
216	src = mali_c55->base + metering_space_addrs[cfg_space];
217	dst += MALI_C55_1024BIN_HIST_SIZE;
218	length = sizeof(struct mali_c55_stats_buffer) - MALI_C55_1024BIN_HIST_SIZE;
219	memcpy_fromio(dst, src, length);
220}
221
222void mali_c55_stats_fill_buffer(struct mali_c55 *mali_c55,
223				enum mali_c55_config_spaces cfg_space)
224{
225	struct mali_c55_stats *stats = &mali_c55->stats;
226	struct mali_c55_stats_buf *buf = NULL;
227
228	spin_lock(&stats->buffers.lock);
229	if (!list_empty(&stats->buffers.queue)) {
230		buf = list_first_entry(&stats->buffers.queue,
231				       struct mali_c55_stats_buf, queue);
232		list_del(&buf->queue);
233	}
234	spin_unlock(&stats->buffers.lock);
235
236	if (!buf)
237		return;
238
239	buf->vb.sequence = mali_c55->isp.frame_sequence;
240	buf->vb.vb2_buf.timestamp = ktime_get_boottime_ns();
241
242	mali_c55_stats_cpu_read(stats, buf, cfg_space);
243	vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
244}
245
246void mali_c55_unregister_stats(struct mali_c55 *mali_c55)
247{
248	struct mali_c55_stats *stats = &mali_c55->stats;
249
250	if (!video_is_registered(&stats->vdev))
251		return;
252
253	vb2_video_unregister_device(&stats->vdev);
254	media_entity_cleanup(&stats->vdev.entity);
255
256	mutex_destroy(&stats->lock);
257}
258
259int mali_c55_register_stats(struct mali_c55 *mali_c55)
260{
261	struct mali_c55_stats *stats = &mali_c55->stats;
262	struct video_device *vdev = &stats->vdev;
263	struct vb2_queue *vb2q = &stats->queue;
264	int ret;
265
266	mutex_init(&stats->lock);
267	INIT_LIST_HEAD(&stats->buffers.queue);
268	spin_lock_init(&stats->buffers.lock);
269
270	stats->pad.flags = MEDIA_PAD_FL_SINK;
271	ret = media_entity_pads_init(&stats->vdev.entity, 1, &stats->pad);
272	if (ret)
273		goto err_destroy_mutex;
274
275	vb2q->type = V4L2_BUF_TYPE_META_CAPTURE;
276	vb2q->io_modes = VB2_MMAP | VB2_DMABUF;
277	vb2q->drv_priv = stats;
278	vb2q->mem_ops = &vb2_dma_contig_memops;
279	vb2q->ops = &mali_c55_stats_vb2_ops;
280	vb2q->buf_struct_size = sizeof(struct mali_c55_stats_buf);
281	vb2q->min_queued_buffers = 1;
282	vb2q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
283	vb2q->lock = &stats->lock;
284	vb2q->dev = mali_c55->dev;
285
286	ret = vb2_queue_init(vb2q);
287	if (ret) {
288		dev_err(mali_c55->dev, "stats vb2 queue init failed\n");
289		goto err_cleanup_entity;
290	}
291
292	strscpy(stats->vdev.name, "mali-c55 3a stats", sizeof(stats->vdev.name));
293	vdev->release = video_device_release_empty;
294	vdev->fops = &mali_c55_stats_v4l2_fops;
295	vdev->ioctl_ops = &mali_c55_stats_v4l2_ioctl_ops;
296	vdev->lock = &stats->lock;
297	vdev->v4l2_dev = &mali_c55->v4l2_dev;
298	vdev->queue = &stats->queue;
299	vdev->device_caps = V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING;
300	vdev->vfl_dir = VFL_DIR_RX;
301	video_set_drvdata(vdev, stats);
302
303	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
304	if (ret) {
305		dev_err(mali_c55->dev,
306			"failed to register stats video device\n");
307		goto err_release_vb2q;
308	}
309
310	stats->mali_c55 = mali_c55;
311
312	return 0;
313
314err_release_vb2q:
315	vb2_queue_release(vb2q);
316err_cleanup_entity:
317	media_entity_cleanup(&stats->vdev.entity);
318err_destroy_mutex:
319
320	mutex_destroy(&stats->lock);
321
322	return ret;
323}