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 28 static const unsigned int metering_space_addrs[] = { 29 [MALI_C55_CONFIG_PING] = 0x095ac, 30 [MALI_C55_CONFIG_PONG] = 0x2156c, 31 }; 32 33 static 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 44 static 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 57 static 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 66 static 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 85 static 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 94 static int 95 mali_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 113 static 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 129 static 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 142 static 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 167 err_stop_pipeline: 168 video_device_pipeline_stop(&stats->vdev); 169 err_pm_put: 170 pm_runtime_put_autosuspend(mali_c55->dev); 171 err_return_buffers: 172 mali_c55_stats_return_buffers(stats, VB2_BUF_STATE_QUEUED); 173 174 return ret; 175 } 176 177 static 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 196 static 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 203 static 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 222 void 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 246 void 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 259 int 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 314 err_release_vb2q: 315 vb2_queue_release(vb2q); 316 err_cleanup_entity: 317 media_entity_cleanup(&stats->vdev.entity); 318 err_destroy_mutex: 319 320 mutex_destroy(&stats->lock); 321 322 return ret; 323 } 324