Branch data Line data Source code
1 : : // SPDX-License-Identifier: GPL-2.0
2 : : /*
3 : : * Broadcom BCM2835 ISP driver
4 : : *
5 : : * Copyright © 2019-2020 Raspberry Pi (Trading) Ltd.
6 : : *
7 : : * Author: Naushir Patuck (naush@raspberrypi.com)
8 : : *
9 : : */
10 : :
11 : : #include <linux/module.h>
12 : : #include <linux/platform_device.h>
13 : :
14 : : #include <media/v4l2-ctrls.h>
15 : : #include <media/v4l2-device.h>
16 : : #include <media/v4l2-event.h>
17 : : #include <media/v4l2-ioctl.h>
18 : : #include <media/videobuf2-dma-contig.h>
19 : :
20 : : #include "vchiq-mmal/mmal-msg.h"
21 : : #include "vchiq-mmal/mmal-parameters.h"
22 : : #include "vchiq-mmal/mmal-vchiq.h"
23 : :
24 : : #include "vc-sm-cma/vc_sm_knl.h"
25 : :
26 : : #include "bcm2835_isp_ctrls.h"
27 : : #include "bcm2835_isp_fmts.h"
28 : :
29 : : static unsigned int debug;
30 : : module_param(debug, uint, 0644);
31 : : MODULE_PARM_DESC(debug, "activates debug info");
32 : :
33 : : static unsigned int video_nr = 13;
34 : : module_param(video_nr, uint, 0644);
35 : : MODULE_PARM_DESC(video_nr, "base video device number");
36 : :
37 : : #define BCM2835_ISP_NAME "bcm2835-isp"
38 : : #define BCM2835_ISP_ENTITY_NAME_LEN 32
39 : :
40 : : #define BCM2835_ISP_NUM_OUTPUTS 1
41 : : #define BCM2835_ISP_NUM_CAPTURES 2
42 : : #define BCM2835_ISP_NUM_METADATA 1
43 : :
44 : : #define BCM2835_ISP_NUM_NODES \
45 : : (BCM2835_ISP_NUM_OUTPUTS + BCM2835_ISP_NUM_CAPTURES + \
46 : : BCM2835_ISP_NUM_METADATA)
47 : :
48 : : /* Default frame dimension of 1280 pixels. */
49 : : #define DEFAULT_DIM 1280U
50 : : /*
51 : : * Maximum frame dimension of 16384 pixels. Even though the ISP runs in tiles,
52 : : * have a sensible limit so that we do not create an excessive number of tiles
53 : : * to process.
54 : : */
55 : : #define MAX_DIM 16384U
56 : : /*
57 : : * Minimum frame dimension of 64 pixels. Anything lower, and the tiling
58 : : * algorihtm may not be able to cope when applying filter context.
59 : : */
60 : : #define MIN_DIM 64U
61 : :
62 : : /* Per-queue, driver-specific private data */
63 : : struct bcm2835_isp_q_data {
64 : : /*
65 : : * These parameters should be treated as gospel, with everything else
66 : : * being determined from them.
67 : : */
68 : : unsigned int bytesperline;
69 : : unsigned int width;
70 : : unsigned int height;
71 : : unsigned int sizeimage;
72 : : const struct bcm2835_isp_fmt *fmt;
73 : : };
74 : :
75 : : /*
76 : : * Structure to describe a single node /dev/video<N> which represents a single
77 : : * input or output queue to the ISP device.
78 : : */
79 : : struct bcm2835_isp_node {
80 : : int vfl_dir;
81 : : unsigned int id;
82 : : const char *name;
83 : : struct video_device vfd;
84 : : struct media_pad pad;
85 : : struct media_intf_devnode *intf_devnode;
86 : : struct media_link *intf_link;
87 : : struct mutex lock; /* top level device node lock */
88 : : struct mutex queue_lock;
89 : :
90 : : struct vb2_queue queue;
91 : : unsigned int sequence;
92 : :
93 : : /* The list of formats supported on the node. */
94 : : struct bcm2835_isp_fmt_list supported_fmts;
95 : :
96 : : struct bcm2835_isp_q_data q_data;
97 : :
98 : : /* Parent device structure */
99 : : struct bcm2835_isp_dev *dev;
100 : :
101 : : bool registered;
102 : : bool media_node_registered;
103 : : bool queue_init;
104 : : };
105 : :
106 : : /*
107 : : * Structure representing the entire ISP device, comprising several input and
108 : : * output nodes /dev/video<N>.
109 : : */
110 : : struct bcm2835_isp_dev {
111 : : struct v4l2_device v4l2_dev;
112 : : struct device *dev;
113 : : struct v4l2_ctrl_handler ctrl_handler;
114 : : struct media_device mdev;
115 : : struct media_entity entity;
116 : : bool media_device_registered;
117 : : bool media_entity_registered;
118 : : struct vchiq_mmal_instance *mmal_instance;
119 : : struct vchiq_mmal_component *component;
120 : : struct completion frame_cmplt;
121 : :
122 : : struct bcm2835_isp_node node[BCM2835_ISP_NUM_NODES];
123 : : struct media_pad pad[BCM2835_ISP_NUM_NODES];
124 : : atomic_t num_streaming;
125 : :
126 : : /* Image pipeline controls. */
127 : : int r_gain;
128 : : int b_gain;
129 : : };
130 : :
131 : : struct bcm2835_isp_buffer {
132 : : struct vb2_v4l2_buffer vb;
133 : : struct mmal_buffer mmal;
134 : : };
135 : :
136 : : static
137 : : inline struct bcm2835_isp_dev *node_get_dev(struct bcm2835_isp_node *node)
138 : : {
139 : 3 : return node->dev;
140 : : }
141 : :
142 : : static inline bool node_is_output(struct bcm2835_isp_node *node)
143 : : {
144 : 3 : return node->queue.type == V4L2_BUF_TYPE_VIDEO_OUTPUT;
145 : : }
146 : :
147 : : static inline bool node_is_capture(struct bcm2835_isp_node *node)
148 : : {
149 : : return node->queue.type == V4L2_BUF_TYPE_VIDEO_CAPTURE;
150 : : }
151 : :
152 : : static inline bool node_is_stats(struct bcm2835_isp_node *node)
153 : : {
154 : 3 : return node->queue.type == V4L2_BUF_TYPE_META_CAPTURE;
155 : : }
156 : :
157 : : static inline enum v4l2_buf_type index_to_queue_type(int index)
158 : : {
159 : 3 : if (index < BCM2835_ISP_NUM_OUTPUTS)
160 : : return V4L2_BUF_TYPE_VIDEO_OUTPUT;
161 : 3 : else if (index < BCM2835_ISP_NUM_OUTPUTS + BCM2835_ISP_NUM_CAPTURES)
162 : : return V4L2_BUF_TYPE_VIDEO_CAPTURE;
163 : : else
164 : : return V4L2_BUF_TYPE_META_CAPTURE;
165 : : }
166 : :
167 : 3 : static struct vchiq_mmal_port *get_port_data(struct bcm2835_isp_node *node)
168 : : {
169 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
170 : :
171 : 3 : if (!dev->component)
172 : : return NULL;
173 : :
174 : 3 : switch (node->queue.type) {
175 : : case V4L2_BUF_TYPE_VIDEO_OUTPUT:
176 : 3 : return &dev->component->input[node->id];
177 : : case V4L2_BUF_TYPE_VIDEO_CAPTURE:
178 : : case V4L2_BUF_TYPE_META_CAPTURE:
179 : 3 : return &dev->component->output[node->id];
180 : : default:
181 : 0 : v4l2_err(&dev->v4l2_dev, "%s: Invalid queue type %u\n",
182 : : __func__, node->queue.type);
183 : : break;
184 : : }
185 : 0 : return NULL;
186 : : }
187 : :
188 : 0 : static int set_isp_param(struct bcm2835_isp_node *node, u32 parameter,
189 : : void *value, u32 value_size)
190 : : {
191 : 0 : struct vchiq_mmal_port *port = get_port_data(node);
192 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
193 : :
194 : 0 : return vchiq_mmal_port_parameter_set(dev->mmal_instance, port,
195 : : parameter, value, value_size);
196 : : }
197 : :
198 : 0 : static int set_wb_gains(struct bcm2835_isp_node *node)
199 : : {
200 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
201 : 0 : struct mmal_parameter_awbgains gains = {
202 : 0 : .r_gain = { dev->r_gain, 1000 },
203 : 0 : .b_gain = { dev->b_gain, 1000 }
204 : : };
205 : :
206 : 0 : return set_isp_param(node, MMAL_PARAMETER_CUSTOM_AWB_GAINS,
207 : : &gains, sizeof(gains));
208 : : }
209 : :
210 : : static int set_digital_gain(struct bcm2835_isp_node *node, uint32_t gain)
211 : : {
212 : 0 : struct mmal_parameter_rational digital_gain = {
213 : : .num = gain,
214 : : .den = 1000
215 : : };
216 : :
217 : 0 : return set_isp_param(node, MMAL_PARAMETER_DIGITAL_GAIN,
218 : : &digital_gain, sizeof(digital_gain));
219 : : }
220 : :
221 : : static const struct bcm2835_isp_fmt *get_fmt(u32 mmal_fmt)
222 : : {
223 : : unsigned int i;
224 : :
225 : 3 : for (i = 0; i < ARRAY_SIZE(supported_formats); i++) {
226 : 3 : if (supported_formats[i].mmal_fmt == mmal_fmt)
227 : 3 : return &supported_formats[i];
228 : : }
229 : : return NULL;
230 : : }
231 : :
232 : : static const
233 : : struct bcm2835_isp_fmt *find_format_by_fourcc(unsigned int fourcc,
234 : : struct bcm2835_isp_node *node)
235 : : {
236 : : struct bcm2835_isp_fmt_list *fmts = &node->supported_fmts;
237 : : const struct bcm2835_isp_fmt *fmt;
238 : : unsigned int i;
239 : :
240 : 0 : for (i = 0; i < fmts->num_entries; i++) {
241 : 0 : fmt = fmts->list[i];
242 : 0 : if (fmt->fourcc == fourcc)
243 : 0 : return fmt;
244 : : }
245 : :
246 : : return NULL;
247 : : }
248 : :
249 : : static const
250 : : struct bcm2835_isp_fmt *find_format(struct v4l2_format *f,
251 : : struct bcm2835_isp_node *node)
252 : : {
253 : 0 : return find_format_by_fourcc(node_is_stats(node) ?
254 : : f->fmt.meta.dataformat :
255 : : f->fmt.pix.pixelformat,
256 : : node);
257 : : }
258 : :
259 : : /* vb2_to_mmal_buffer() - converts vb2 buffer header to MMAL
260 : : *
261 : : * Copies all the required fields from a VB2 buffer to the MMAL buffer header,
262 : : * ready for sending to the VPU.
263 : : */
264 : 0 : static void vb2_to_mmal_buffer(struct mmal_buffer *buf,
265 : : struct vb2_v4l2_buffer *vb2)
266 : : {
267 : : u64 pts;
268 : :
269 : 0 : buf->mmal_flags = 0;
270 : 0 : if (vb2->flags & V4L2_BUF_FLAG_KEYFRAME)
271 : 0 : buf->mmal_flags |= MMAL_BUFFER_HEADER_FLAG_KEYFRAME;
272 : :
273 : : /* Data must be framed correctly as one frame per buffer. */
274 : 0 : buf->mmal_flags |= MMAL_BUFFER_HEADER_FLAG_FRAME_END;
275 : :
276 : 0 : buf->length = vb2->vb2_buf.planes[0].bytesused;
277 : : /*
278 : : * Minor ambiguity in the V4L2 spec as to whether passing in a 0 length
279 : : * buffer, or one with V4L2_BUF_FLAG_LAST set denotes end of stream.
280 : : * Handle either.
281 : : */
282 : 0 : if (!buf->length || vb2->flags & V4L2_BUF_FLAG_LAST)
283 : 0 : buf->mmal_flags |= MMAL_BUFFER_HEADER_FLAG_EOS;
284 : :
285 : : /* vb2 timestamps in nsecs, mmal in usecs */
286 : 0 : pts = vb2->vb2_buf.timestamp;
287 : 0 : do_div(pts, 1000);
288 : 0 : buf->pts = pts;
289 : 0 : buf->dts = MMAL_TIME_UNKNOWN;
290 : 0 : }
291 : :
292 : 0 : static void mmal_buffer_cb(struct vchiq_mmal_instance *instance,
293 : : struct vchiq_mmal_port *port, int status,
294 : : struct mmal_buffer *mmal_buf)
295 : : {
296 : : struct bcm2835_isp_buffer *q_buf;
297 : 0 : struct bcm2835_isp_node *node = port->cb_ctx;
298 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
299 : : struct vb2_v4l2_buffer *vb2;
300 : :
301 : : q_buf = container_of(mmal_buf, struct bcm2835_isp_buffer, mmal);
302 : 0 : vb2 = &q_buf->vb;
303 : 0 : v4l2_dbg(2, debug, &dev->v4l2_dev,
304 : : "%s: port:%s[%d], status:%d, buf:%p, dmabuf:%p, length:%lu, flags %u, pts %lld\n",
305 : : __func__, node_is_output(node) ? "input" : "output", node->id,
306 : : status, mmal_buf, mmal_buf->dma_buf, mmal_buf->length,
307 : : mmal_buf->mmal_flags, mmal_buf->pts);
308 : :
309 : 0 : if (mmal_buf->cmd)
310 : 0 : v4l2_err(&dev->v4l2_dev,
311 : : "%s: Unexpected event on output callback - %08x\n",
312 : : __func__, mmal_buf->cmd);
313 : :
314 : 0 : if (status) {
315 : : /* error in transfer */
316 : 0 : if (vb2) {
317 : : /* there was a buffer with the error so return it */
318 : 0 : vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_ERROR);
319 : : }
320 : 0 : return;
321 : : }
322 : :
323 : : /* vb2 timestamps in nsecs, mmal in usecs */
324 : 0 : vb2->vb2_buf.timestamp = mmal_buf->pts * 1000;
325 : 0 : vb2->sequence = node->sequence++;
326 : 0 : vb2_set_plane_payload(&vb2->vb2_buf, 0, mmal_buf->length);
327 : 0 : vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_DONE);
328 : :
329 : 0 : if (!port->enabled)
330 : 0 : complete(&dev->frame_cmplt);
331 : : }
332 : :
333 : : static void setup_mmal_port_format(struct bcm2835_isp_node *node,
334 : : struct vchiq_mmal_port *port)
335 : : {
336 : : struct bcm2835_isp_q_data *q_data = &node->q_data;
337 : :
338 : 0 : port->format.encoding = q_data->fmt->mmal_fmt;
339 : : /* Raw image format - set width/height */
340 : 0 : port->es.video.width = (q_data->bytesperline << 3) / q_data->fmt->depth;
341 : 0 : port->es.video.height = q_data->height;
342 : 0 : port->es.video.crop.width = q_data->width;
343 : 0 : port->es.video.crop.height = q_data->height;
344 : 0 : port->es.video.crop.x = 0;
345 : 0 : port->es.video.crop.y = 0;
346 : : };
347 : :
348 : 0 : static int setup_mmal_port(struct bcm2835_isp_node *node)
349 : : {
350 : 0 : struct vchiq_mmal_port *port = get_port_data(node);
351 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
352 : 0 : unsigned int enable = 1;
353 : : int ret;
354 : :
355 : 0 : v4l2_dbg(2, debug, &dev->v4l2_dev, "%s: setup %s[%d]\n", __func__,
356 : : node->name, node->id);
357 : :
358 : 0 : vchiq_mmal_port_parameter_set(dev->mmal_instance, port,
359 : : MMAL_PARAMETER_ZERO_COPY, &enable,
360 : : sizeof(enable));
361 : : setup_mmal_port_format(node, port);
362 : 0 : ret = vchiq_mmal_port_set_format(dev->mmal_instance, port);
363 : 0 : if (ret < 0) {
364 : 0 : v4l2_dbg(1, debug, &dev->v4l2_dev,
365 : : "%s: vchiq_mmal_port_set_format failed\n",
366 : : __func__);
367 : 0 : return ret;
368 : : }
369 : :
370 : 0 : if (node->q_data.sizeimage < port->minimum_buffer.size) {
371 : 0 : v4l2_err(&dev->v4l2_dev,
372 : : "buffer size mismatch sizeimage %u < min size %u\n",
373 : : node->q_data.sizeimage, port->minimum_buffer.size);
374 : 0 : return -EINVAL;
375 : : }
376 : :
377 : : return 0;
378 : : }
379 : :
380 : 0 : static int bcm2835_isp_mmal_buf_cleanup(struct mmal_buffer *mmal_buf)
381 : : {
382 : 0 : mmal_vchi_buffer_cleanup(mmal_buf);
383 : :
384 : 0 : if (mmal_buf->dma_buf) {
385 : 0 : dma_buf_put(mmal_buf->dma_buf);
386 : 0 : mmal_buf->dma_buf = NULL;
387 : : }
388 : :
389 : 0 : return 0;
390 : : }
391 : :
392 : 0 : static int bcm2835_isp_node_queue_setup(struct vb2_queue *q,
393 : : unsigned int *nbuffers,
394 : : unsigned int *nplanes,
395 : : unsigned int sizes[],
396 : : struct device *alloc_devs[])
397 : : {
398 : : struct bcm2835_isp_node *node = vb2_get_drv_priv(q);
399 : : struct vchiq_mmal_port *port;
400 : : unsigned int size;
401 : :
402 : 0 : if (setup_mmal_port(node))
403 : : return -EINVAL;
404 : :
405 : 0 : size = node->q_data.sizeimage;
406 : 0 : if (size == 0) {
407 : 0 : v4l2_info(&node_get_dev(node)->v4l2_dev,
408 : : "%s: Image size unset in queue_setup for node %s[%d]\n",
409 : : __func__, node->name, node->id);
410 : 0 : return -EINVAL;
411 : : }
412 : :
413 : 0 : if (*nplanes)
414 : 0 : return sizes[0] < size ? -EINVAL : 0;
415 : :
416 : 0 : *nplanes = 1;
417 : 0 : sizes[0] = size;
418 : :
419 : 0 : port = get_port_data(node);
420 : 0 : port->current_buffer.size = size;
421 : :
422 : 0 : if (*nbuffers < port->minimum_buffer.num)
423 : 0 : *nbuffers = port->minimum_buffer.num;
424 : :
425 : 0 : port->current_buffer.num = *nbuffers;
426 : :
427 : 0 : v4l2_dbg(2, debug, &node_get_dev(node)->v4l2_dev,
428 : : "%s: Image size %u, nbuffers %u for node %s[%d]\n",
429 : : __func__, sizes[0], *nbuffers, node->name, node->id);
430 : : return 0;
431 : : }
432 : :
433 : 0 : static int bcm2835_isp_buf_init(struct vb2_buffer *vb)
434 : : {
435 : 0 : struct bcm2835_isp_node *node = vb2_get_drv_priv(vb->vb2_queue);
436 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
437 : : struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
438 : : struct bcm2835_isp_buffer *buf =
439 : : container_of(vb2, struct bcm2835_isp_buffer, vb);
440 : :
441 : 0 : v4l2_dbg(3, debug, &dev->v4l2_dev, "%s: vb %p\n", __func__, vb);
442 : :
443 : 0 : buf->mmal.buffer = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
444 : 0 : buf->mmal.buffer_size = vb2_plane_size(&buf->vb.vb2_buf, 0);
445 : 0 : mmal_vchi_buffer_init(dev->mmal_instance, &buf->mmal);
446 : 0 : return 0;
447 : : }
448 : :
449 : 0 : static int bcm2835_isp_buf_prepare(struct vb2_buffer *vb)
450 : : {
451 : 0 : struct bcm2835_isp_node *node = vb2_get_drv_priv(vb->vb2_queue);
452 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
453 : : struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
454 : : struct bcm2835_isp_buffer *buf =
455 : : container_of(vb2, struct bcm2835_isp_buffer, vb);
456 : : struct dma_buf *dma_buf;
457 : : int ret;
458 : :
459 : 0 : v4l2_dbg(3, debug, &dev->v4l2_dev, "%s: type: %d ptr %p\n",
460 : : __func__, vb->vb2_queue->type, vb);
461 : :
462 : 0 : if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
463 : 0 : if (vb2->field == V4L2_FIELD_ANY)
464 : 0 : vb2->field = V4L2_FIELD_NONE;
465 : 0 : if (vb2->field != V4L2_FIELD_NONE) {
466 : 0 : v4l2_err(&dev->v4l2_dev,
467 : : "%s field isn't supported\n", __func__);
468 : 0 : return -EINVAL;
469 : : }
470 : : }
471 : :
472 : 0 : if (vb2_plane_size(vb, 0) < node->q_data.sizeimage) {
473 : 0 : v4l2_err(&dev->v4l2_dev,
474 : : "%s data will not fit into plane (%lu < %lu)\n",
475 : : __func__, vb2_plane_size(vb, 0),
476 : : (long)node->q_data.sizeimage);
477 : 0 : return -EINVAL;
478 : : }
479 : :
480 : 0 : if (!V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type))
481 : : vb2_set_plane_payload(vb, 0, node->q_data.sizeimage);
482 : :
483 : 0 : switch (vb->memory) {
484 : : case VB2_MEMORY_DMABUF:
485 : 0 : dma_buf = dma_buf_get(vb->planes[0].m.fd);
486 : :
487 : 0 : if (dma_buf != buf->mmal.dma_buf) {
488 : : /*
489 : : * dmabuf either hasn't already been mapped, or it has
490 : : * changed.
491 : : */
492 : 0 : if (buf->mmal.dma_buf) {
493 : 0 : v4l2_err(&dev->v4l2_dev,
494 : : "%s Buffer changed - why did the core not call cleanup?\n",
495 : : __func__);
496 : 0 : bcm2835_isp_mmal_buf_cleanup(&buf->mmal);
497 : : }
498 : :
499 : 0 : buf->mmal.dma_buf = dma_buf;
500 : : } else {
501 : : /*
502 : : * Already have a reference to the buffer, so release it
503 : : * here.
504 : : */
505 : 0 : dma_buf_put(dma_buf);
506 : : }
507 : : ret = 0;
508 : : break;
509 : : case VB2_MEMORY_MMAP:
510 : : /*
511 : : * We want to do this at init, but vb2_core_expbuf checks that
512 : : * the index < q->num_buffers, and q->num_buffers only gets
513 : : * updated once all the buffers are allocated.
514 : : */
515 : 0 : if (!buf->mmal.dma_buf) {
516 : 0 : ret = vb2_core_expbuf_dmabuf(vb->vb2_queue,
517 : : vb->vb2_queue->type,
518 : : vb->index, 0, O_CLOEXEC,
519 : : &buf->mmal.dma_buf);
520 : 0 : v4l2_dbg(3, debug, &dev->v4l2_dev,
521 : : "%s: exporting ptr %p to dmabuf %p\n",
522 : : __func__, vb, buf->mmal.dma_buf);
523 : 0 : if (ret)
524 : 0 : v4l2_err(&dev->v4l2_dev,
525 : : "%s: Failed to expbuf idx %d, ret %d\n",
526 : : __func__, vb->index, ret);
527 : : } else {
528 : : ret = 0;
529 : : }
530 : : break;
531 : : default:
532 : : ret = -EINVAL;
533 : : break;
534 : : }
535 : :
536 : 0 : return ret;
537 : : }
538 : :
539 : 0 : static void bcm2835_isp_node_buffer_queue(struct vb2_buffer *buf)
540 : : {
541 : 0 : struct bcm2835_isp_node *node = vb2_get_drv_priv(buf->vb2_queue);
542 : : struct vb2_v4l2_buffer *vbuf =
543 : : container_of(buf, struct vb2_v4l2_buffer, vb2_buf);
544 : : struct bcm2835_isp_buffer *buffer =
545 : : container_of(vbuf, struct bcm2835_isp_buffer, vb);
546 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
547 : :
548 : 0 : v4l2_dbg(3, debug, &dev->v4l2_dev, "%s: node %s[%d], buffer %p\n",
549 : : __func__, node->name, node->id, buffer);
550 : :
551 : 0 : vb2_to_mmal_buffer(&buffer->mmal, &buffer->vb);
552 : 0 : v4l2_dbg(3, debug, &dev->v4l2_dev,
553 : : "%s: node %s[%d] - submitting mmal dmabuf %p\n", __func__,
554 : : node->name, node->id, buffer->mmal.dma_buf);
555 : 0 : vchiq_mmal_submit_buffer(dev->mmal_instance, get_port_data(node),
556 : : &buffer->mmal);
557 : 0 : }
558 : :
559 : 0 : static void bcm2835_isp_buffer_cleanup(struct vb2_buffer *vb)
560 : : {
561 : : struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
562 : : struct bcm2835_isp_buffer *buffer =
563 : : container_of(vb2, struct bcm2835_isp_buffer, vb);
564 : :
565 : 0 : bcm2835_isp_mmal_buf_cleanup(&buffer->mmal);
566 : 0 : }
567 : :
568 : 0 : static int bcm2835_isp_node_start_streaming(struct vb2_queue *q,
569 : : unsigned int count)
570 : : {
571 : : struct bcm2835_isp_node *node = vb2_get_drv_priv(q);
572 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
573 : 0 : struct vchiq_mmal_port *port = get_port_data(node);
574 : : int ret;
575 : :
576 : 0 : v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: node %s[%d] (count %u)\n",
577 : : __func__, node->name, node->id, count);
578 : :
579 : 0 : ret = vchiq_mmal_component_enable(dev->mmal_instance, dev->component);
580 : 0 : if (ret) {
581 : 0 : v4l2_err(&dev->v4l2_dev, "%s: Failed enabling component, ret %d\n",
582 : : __func__, ret);
583 : 0 : return -EIO;
584 : : }
585 : :
586 : 0 : node->sequence = 0;
587 : 0 : port->cb_ctx = node;
588 : 0 : ret = vchiq_mmal_port_enable(dev->mmal_instance, port,
589 : : mmal_buffer_cb);
590 : 0 : if (!ret)
591 : 0 : atomic_inc(&dev->num_streaming);
592 : : else
593 : 0 : v4l2_err(&dev->v4l2_dev,
594 : : "%s: Failed enabling port, ret %d\n", __func__, ret);
595 : :
596 : 0 : return ret;
597 : : }
598 : :
599 : 0 : static void bcm2835_isp_node_stop_streaming(struct vb2_queue *q)
600 : : {
601 : : struct bcm2835_isp_node *node = vb2_get_drv_priv(q);
602 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
603 : 0 : struct vchiq_mmal_port *port = get_port_data(node);
604 : : unsigned int i;
605 : : int ret;
606 : :
607 : 0 : v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: node %s[%d], mmal port %p\n",
608 : : __func__, node->name, node->id, port);
609 : :
610 : : init_completion(&dev->frame_cmplt);
611 : :
612 : : /* Disable MMAL port - this will flush buffers back */
613 : 0 : ret = vchiq_mmal_port_disable(dev->mmal_instance, port);
614 : 0 : if (ret)
615 : 0 : v4l2_err(&dev->v4l2_dev,
616 : : "%s: Failed disabling %s port, ret %d\n", __func__,
617 : : node_is_output(node) ? "i/p" : "o/p",
618 : : ret);
619 : :
620 : 0 : while (atomic_read(&port->buffers_with_vpu)) {
621 : 0 : v4l2_dbg(1, debug, &dev->v4l2_dev,
622 : : "%s: Waiting for buffers to be returned - %d outstanding\n",
623 : : __func__, atomic_read(&port->buffers_with_vpu));
624 : 0 : ret = wait_for_completion_timeout(&dev->frame_cmplt, HZ);
625 : 0 : if (ret <= 0) {
626 : 0 : v4l2_err(&dev->v4l2_dev,
627 : : "%s: Timeout waiting for buffers to be returned - %d outstanding\n",
628 : : __func__,
629 : : atomic_read(&port->buffers_with_vpu));
630 : 0 : break;
631 : : }
632 : : }
633 : :
634 : : /* Release the VCSM handle here to release the associated dmabuf */
635 : 0 : for (i = 0; i < q->num_buffers; i++) {
636 : 0 : struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(q->bufs[i]);
637 : : struct bcm2835_isp_buffer *buf =
638 : : container_of(vb2, struct bcm2835_isp_buffer, vb);
639 : 0 : bcm2835_isp_mmal_buf_cleanup(&buf->mmal);
640 : : }
641 : :
642 : 0 : atomic_dec(&dev->num_streaming);
643 : : /* If all ports disabled, then disable the component */
644 : 0 : if (atomic_read(&dev->num_streaming) == 0) {
645 : 0 : ret = vchiq_mmal_component_disable(dev->mmal_instance,
646 : : dev->component);
647 : 0 : if (ret) {
648 : 0 : v4l2_err(&dev->v4l2_dev,
649 : : "%s: Failed disabling component, ret %d\n",
650 : : __func__, ret);
651 : : }
652 : : }
653 : :
654 : : /*
655 : : * Simply wait for any vb2 buffers to finish. We could take steps to
656 : : * make them complete more quickly if we care, or even return them
657 : : * ourselves.
658 : : */
659 : 0 : vb2_wait_for_all_buffers(&node->queue);
660 : 0 : }
661 : :
662 : : static const struct vb2_ops bcm2835_isp_node_queue_ops = {
663 : : .queue_setup = bcm2835_isp_node_queue_setup,
664 : : .buf_init = bcm2835_isp_buf_init,
665 : : .buf_prepare = bcm2835_isp_buf_prepare,
666 : : .buf_queue = bcm2835_isp_node_buffer_queue,
667 : : .buf_cleanup = bcm2835_isp_buffer_cleanup,
668 : : .start_streaming = bcm2835_isp_node_start_streaming,
669 : : .stop_streaming = bcm2835_isp_node_stop_streaming,
670 : : };
671 : :
672 : : static const
673 : : struct bcm2835_isp_fmt *get_default_format(struct bcm2835_isp_node *node)
674 : : {
675 : 3 : return node->supported_fmts.list[0];
676 : : }
677 : :
678 : : static inline unsigned int get_bytesperline(int width,
679 : : const struct bcm2835_isp_fmt *fmt)
680 : : {
681 : : /* GPU aligns 24bpp images to a multiple of 32 pixels (not bytes). */
682 : 3 : if (fmt->depth == 24)
683 : 0 : return ALIGN(width, 32) * 3;
684 : : else
685 : 3 : return ALIGN((width * fmt->depth) >> 3, fmt->bytesperline_align);
686 : : }
687 : :
688 : : static inline unsigned int get_sizeimage(int bpl, int width, int height,
689 : : const struct bcm2835_isp_fmt *fmt)
690 : : {
691 : 3 : return (bpl * height * fmt->size_multiplier_x2) >> 1;
692 : : }
693 : :
694 : 0 : static int bcm2835_isp_s_ctrl(struct v4l2_ctrl *ctrl)
695 : : {
696 : : struct bcm2835_isp_dev *dev =
697 : 0 : container_of(ctrl->handler, struct bcm2835_isp_dev, ctrl_handler);
698 : 0 : struct bcm2835_isp_node *node = &dev->node[0];
699 : : int ret = 0;
700 : :
701 : : /*
702 : : * The ISP firmware driver will ensure these settings are applied on
703 : : * a frame boundary, so we are safe to write them as they come in.
704 : : *
705 : : * Note that the bcm2835_isp_* param structures are identical to the
706 : : * mmal-parameters.h definitions. This avoids the need for unnecessary
707 : : * field-by-field copying between structures.
708 : : */
709 : 0 : switch (ctrl->id) {
710 : : case V4L2_CID_RED_BALANCE:
711 : 0 : dev->r_gain = ctrl->val;
712 : 0 : ret = set_wb_gains(node);
713 : 0 : break;
714 : : case V4L2_CID_BLUE_BALANCE:
715 : 0 : dev->b_gain = ctrl->val;
716 : 0 : ret = set_wb_gains(node);
717 : 0 : break;
718 : : case V4L2_CID_DIGITAL_GAIN:
719 : 0 : ret = set_digital_gain(node, ctrl->val);
720 : 0 : break;
721 : : case V4L2_CID_USER_BCM2835_ISP_CC_MATRIX:
722 : 0 : ret = set_isp_param(node, MMAL_PARAMETER_CUSTOM_CCM,
723 : 0 : ctrl->p_new.p_u8,
724 : : sizeof(struct bcm2835_isp_custom_ccm));
725 : 0 : break;
726 : : case V4L2_CID_USER_BCM2835_ISP_LENS_SHADING:
727 : : {
728 : : struct bcm2835_isp_lens_shading ls;
729 : : struct dma_buf *dmabuf;
730 : : void *vcsm_handle;
731 : :
732 : 0 : memcpy(&ls, ctrl->p_new.p_u8,
733 : : sizeof(struct bcm2835_isp_lens_shading));
734 : :
735 : 0 : dmabuf = dma_buf_get(ls.dmabuf);
736 : 0 : if (IS_ERR_OR_NULL(dmabuf))
737 : 0 : return -EINVAL;
738 : :
739 : 0 : ret = vc_sm_cma_import_dmabuf(dmabuf,
740 : : &vcsm_handle);
741 : 0 : if (ret) {
742 : 0 : dma_buf_put(dmabuf);
743 : 0 : return -EINVAL;
744 : : }
745 : :
746 : 0 : ls.dmabuf = vc_sm_cma_int_handle(vcsm_handle);
747 : 0 : if (ls.dmabuf)
748 : 0 : ret = set_isp_param(node,
749 : : MMAL_PARAMETER_LENS_SHADING_OVERRIDE,
750 : : &ls,
751 : : sizeof(struct bcm2835_isp_lens_shading));
752 : : else
753 : : ret = -EINVAL;
754 : :
755 : 0 : vc_sm_cma_free(vcsm_handle);
756 : 0 : dma_buf_put(dmabuf);
757 : 0 : break;
758 : : }
759 : : case V4L2_CID_USER_BCM2835_ISP_BLACK_LEVEL:
760 : 0 : ret = set_isp_param(node, MMAL_PARAMETER_BLACK_LEVEL,
761 : 0 : ctrl->p_new.p_u8,
762 : : sizeof(struct bcm2835_isp_black_level));
763 : 0 : break;
764 : : case V4L2_CID_USER_BCM2835_ISP_GEQ:
765 : 0 : ret = set_isp_param(node, MMAL_PARAMETER_GEQ,
766 : 0 : ctrl->p_new.p_u8,
767 : : sizeof(struct bcm2835_isp_geq));
768 : 0 : break;
769 : : case V4L2_CID_USER_BCM2835_ISP_GAMMA:
770 : 0 : ret = set_isp_param(node, MMAL_PARAMETER_GAMMA,
771 : 0 : ctrl->p_new.p_u8,
772 : : sizeof(struct bcm2835_isp_gamma));
773 : 0 : break;
774 : : case V4L2_CID_USER_BCM2835_ISP_DENOISE:
775 : 0 : ret = set_isp_param(node, MMAL_PARAMETER_DENOISE,
776 : 0 : ctrl->p_new.p_u8,
777 : : sizeof(struct bcm2835_isp_denoise));
778 : 0 : break;
779 : : case V4L2_CID_USER_BCM2835_ISP_SHARPEN:
780 : 0 : ret = set_isp_param(node, MMAL_PARAMETER_SHARPEN,
781 : 0 : ctrl->p_new.p_u8,
782 : : sizeof(struct bcm2835_isp_sharpen));
783 : 0 : break;
784 : : case V4L2_CID_USER_BCM2835_ISP_DPC:
785 : 0 : ret = set_isp_param(node, MMAL_PARAMETER_DPC,
786 : 0 : ctrl->p_new.p_u8,
787 : : sizeof(struct bcm2835_isp_dpc));
788 : 0 : break;
789 : : default:
790 : 0 : v4l2_info(&dev->v4l2_dev, "Unrecognised control\n");
791 : : ret = -EINVAL;
792 : : }
793 : :
794 : 0 : if (ret) {
795 : 0 : v4l2_err(&dev->v4l2_dev, "%s: Failed setting ctrl \"%s\" (%08x), err %d\n",
796 : : __func__, ctrl->name, ctrl->id, ret);
797 : : ret = -EIO;
798 : : }
799 : :
800 : 0 : return ret;
801 : : }
802 : :
803 : : static const struct v4l2_ctrl_ops bcm2835_isp_ctrl_ops = {
804 : : .s_ctrl = bcm2835_isp_s_ctrl,
805 : : };
806 : :
807 : : static const struct v4l2_file_operations bcm2835_isp_fops = {
808 : : .owner = THIS_MODULE,
809 : : .open = v4l2_fh_open,
810 : : .release = vb2_fop_release,
811 : : .poll = vb2_fop_poll,
812 : : .unlocked_ioctl = video_ioctl2,
813 : : .mmap = vb2_fop_mmap
814 : : };
815 : :
816 : 0 : static int populate_qdata_fmt(struct v4l2_format *f,
817 : : struct bcm2835_isp_node *node)
818 : : {
819 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
820 : : struct bcm2835_isp_q_data *q_data = &node->q_data;
821 : : struct vchiq_mmal_port *port;
822 : : int ret;
823 : :
824 : 0 : if (!node_is_stats(node)) {
825 : 0 : v4l2_dbg(1, debug, &dev->v4l2_dev,
826 : : "%s: Setting pix format for type %d, wxh: %ux%u, fmt: %08x, size %u\n",
827 : : __func__, f->type, f->fmt.pix.width, f->fmt.pix.height,
828 : : f->fmt.pix.pixelformat, f->fmt.pix.sizeimage);
829 : :
830 : 0 : q_data->fmt = find_format(f, node);
831 : 0 : q_data->width = f->fmt.pix.width;
832 : 0 : q_data->height = f->fmt.pix.height;
833 : 0 : q_data->height = f->fmt.pix.height;
834 : :
835 : : /* All parameters should have been set correctly by try_fmt */
836 : 0 : q_data->bytesperline = f->fmt.pix.bytesperline;
837 : 0 : q_data->sizeimage = f->fmt.pix.sizeimage;
838 : : } else {
839 : 0 : v4l2_dbg(1, debug, &dev->v4l2_dev,
840 : : "%s: Setting meta format for fmt: %08x, size %u\n",
841 : : __func__, f->fmt.meta.dataformat,
842 : : f->fmt.meta.buffersize);
843 : :
844 : 0 : q_data->fmt = find_format(f, node);
845 : 0 : q_data->width = 0;
846 : 0 : q_data->height = 0;
847 : 0 : q_data->bytesperline = 0;
848 : 0 : q_data->sizeimage = f->fmt.meta.buffersize;
849 : : }
850 : :
851 : 0 : v4l2_dbg(1, debug, &dev->v4l2_dev,
852 : : "%s: Calculated bpl as %u, size %u\n", __func__,
853 : : q_data->bytesperline, q_data->sizeimage);
854 : :
855 : : /* If we have a component then setup the port as well */
856 : 0 : port = get_port_data(node);
857 : : setup_mmal_port_format(node, port);
858 : 0 : ret = vchiq_mmal_port_set_format(dev->mmal_instance, port);
859 : 0 : if (ret) {
860 : 0 : v4l2_err(&dev->v4l2_dev,
861 : : "%s: Failed vchiq_mmal_port_set_format on port, ret %d\n",
862 : : __func__, ret);
863 : : ret = -EINVAL;
864 : : }
865 : :
866 : 0 : if (q_data->sizeimage < port->minimum_buffer.size) {
867 : 0 : v4l2_err(&dev->v4l2_dev,
868 : : "%s: Current buffer size of %u < min buf size %u - driver mismatch to MMAL\n",
869 : : __func__,
870 : : q_data->sizeimage,
871 : : port->minimum_buffer.size);
872 : : }
873 : :
874 : 0 : v4l2_dbg(1, debug, &dev->v4l2_dev,
875 : : "%s: Set format for type %d, wxh: %dx%d, fmt: %08x, size %u\n",
876 : : __func__, f->type, q_data->width, q_data->height,
877 : : q_data->fmt->fourcc, q_data->sizeimage);
878 : :
879 : 0 : return ret;
880 : : }
881 : :
882 : 3 : static int bcm2835_isp_node_querycap(struct file *file, void *priv,
883 : : struct v4l2_capability *cap)
884 : : {
885 : 3 : strscpy(cap->driver, BCM2835_ISP_NAME, sizeof(cap->driver));
886 : 3 : strscpy(cap->card, BCM2835_ISP_NAME, sizeof(cap->card));
887 : 3 : snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
888 : : BCM2835_ISP_NAME);
889 : :
890 : 3 : return 0;
891 : : }
892 : :
893 : 0 : static int bcm2835_isp_node_g_fmt(struct file *file, void *priv,
894 : : struct v4l2_format *f)
895 : : {
896 : : struct bcm2835_isp_node *node = video_drvdata(file);
897 : :
898 : 0 : if (f->type != node->queue.type)
899 : : return -EINVAL;
900 : :
901 : 0 : if (node_is_stats(node)) {
902 : 0 : f->fmt.meta.dataformat = V4L2_META_FMT_BCM2835_ISP_STATS;
903 : 0 : f->fmt.meta.buffersize =
904 : 0 : get_port_data(node)->minimum_buffer.size;
905 : : } else {
906 : : struct bcm2835_isp_q_data *q_data = &node->q_data;
907 : :
908 : 0 : f->fmt.pix.width = q_data->width;
909 : 0 : f->fmt.pix.height = q_data->height;
910 : 0 : f->fmt.pix.field = V4L2_FIELD_NONE;
911 : 0 : f->fmt.pix.pixelformat = q_data->fmt->fourcc;
912 : 0 : f->fmt.pix.bytesperline = q_data->bytesperline;
913 : 0 : f->fmt.pix.sizeimage = q_data->sizeimage;
914 : 0 : f->fmt.pix.colorspace = q_data->fmt->colorspace;
915 : : }
916 : :
917 : : return 0;
918 : : }
919 : :
920 : 0 : static int bcm2835_isp_node_enum_fmt(struct file *file, void *priv,
921 : : struct v4l2_fmtdesc *f)
922 : : {
923 : : struct bcm2835_isp_node *node = video_drvdata(file);
924 : : struct bcm2835_isp_fmt_list *fmts = &node->supported_fmts;
925 : :
926 : 0 : if (f->type != node->queue.type)
927 : : return -EINVAL;
928 : :
929 : 0 : if (f->index < fmts->num_entries) {
930 : : /* Format found */
931 : 0 : f->pixelformat = fmts->list[f->index]->fourcc;
932 : 0 : f->flags = fmts->list[f->index]->flags;
933 : 0 : return 0;
934 : : }
935 : :
936 : : return -EINVAL;
937 : : }
938 : :
939 : 0 : static int bcm2835_isp_enum_framesizes(struct file *file, void *priv,
940 : : struct v4l2_frmsizeenum *fsize)
941 : : {
942 : : struct bcm2835_isp_node *node = video_drvdata(file);
943 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
944 : : const struct bcm2835_isp_fmt *fmt;
945 : :
946 : 0 : if (node_is_stats(node) || fsize->index)
947 : : return -EINVAL;
948 : :
949 : 0 : fmt = find_format_by_fourcc(fsize->pixel_format, node);
950 : 0 : if (!fmt) {
951 : 0 : v4l2_err(&dev->v4l2_dev, "Invalid pixel code: %x\n",
952 : : fsize->pixel_format);
953 : 0 : return -EINVAL;
954 : : }
955 : :
956 : 0 : fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
957 : 0 : fsize->stepwise.min_width = MIN_DIM;
958 : 0 : fsize->stepwise.max_width = MAX_DIM;
959 : 0 : fsize->stepwise.step_width = fmt->step_size;
960 : :
961 : 0 : fsize->stepwise.min_height = MIN_DIM;
962 : 0 : fsize->stepwise.max_height = MAX_DIM;
963 : 0 : fsize->stepwise.step_height = fmt->step_size;
964 : :
965 : 0 : return 0;
966 : : }
967 : :
968 : 0 : static int bcm2835_isp_node_try_fmt(struct file *file, void *priv,
969 : : struct v4l2_format *f)
970 : : {
971 : : struct bcm2835_isp_node *node = video_drvdata(file);
972 : : const struct bcm2835_isp_fmt *fmt;
973 : :
974 : 0 : if (f->type != node->queue.type)
975 : : return -EINVAL;
976 : :
977 : : fmt = find_format(f, node);
978 : 0 : if (!fmt)
979 : : fmt = get_default_format(node);
980 : :
981 : 0 : if (!node_is_stats(node)) {
982 : 0 : f->fmt.pix.width = max(min(f->fmt.pix.width, MAX_DIM),
983 : : MIN_DIM);
984 : 0 : f->fmt.pix.height = max(min(f->fmt.pix.height, MAX_DIM),
985 : : MIN_DIM);
986 : :
987 : 0 : f->fmt.pix.pixelformat = fmt->fourcc;
988 : 0 : f->fmt.pix.colorspace = fmt->colorspace;
989 : 0 : f->fmt.pix.bytesperline = get_bytesperline(f->fmt.pix.width,
990 : : fmt);
991 : 0 : f->fmt.pix.field = V4L2_FIELD_NONE;
992 : 0 : f->fmt.pix.sizeimage =
993 : 0 : get_sizeimage(f->fmt.pix.bytesperline, f->fmt.pix.width,
994 : : f->fmt.pix.height, fmt);
995 : : } else {
996 : 0 : f->fmt.meta.dataformat = fmt->fourcc;
997 : 0 : f->fmt.meta.buffersize =
998 : 0 : get_port_data(node)->minimum_buffer.size;
999 : : }
1000 : :
1001 : : return 0;
1002 : : }
1003 : :
1004 : 0 : static int bcm2835_isp_node_s_fmt(struct file *file, void *priv,
1005 : : struct v4l2_format *f)
1006 : : {
1007 : : struct bcm2835_isp_node *node = video_drvdata(file);
1008 : : int ret;
1009 : :
1010 : 0 : if (f->type != node->queue.type)
1011 : : return -EINVAL;
1012 : :
1013 : 0 : ret = bcm2835_isp_node_try_fmt(file, priv, f);
1014 : 0 : if (ret)
1015 : : return ret;
1016 : :
1017 : 0 : v4l2_dbg(1, debug, &node_get_dev(node)->v4l2_dev,
1018 : : "%s: Set format for node %s[%d]\n",
1019 : : __func__, node->name, node->id);
1020 : :
1021 : 0 : return populate_qdata_fmt(f, node);
1022 : : }
1023 : :
1024 : 0 : static int bcm2835_isp_node_s_selection(struct file *file, void *fh,
1025 : : struct v4l2_selection *s)
1026 : : {
1027 : : struct mmal_parameter_crop crop;
1028 : : struct bcm2835_isp_node *node = video_drvdata(file);
1029 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
1030 : 0 : struct vchiq_mmal_port *port = get_port_data(node);
1031 : :
1032 : : /* This return value is required fro V4L2 compliance. */
1033 : 0 : if (node_is_stats(node))
1034 : : return -ENOTTY;
1035 : :
1036 : 0 : if (!s->r.width || !s->r.height)
1037 : : return -EINVAL;
1038 : :
1039 : : /* We can only set crop on the input. */
1040 : 0 : switch (s->target) {
1041 : : case V4L2_SEL_TGT_CROP:
1042 : : /*
1043 : : * Adjust the crop window if it goes outside of the frame
1044 : : * dimensions.
1045 : : */
1046 : 0 : s->r.left = min((unsigned int)max(s->r.left, 0),
1047 : : node->q_data.width - MIN_DIM);
1048 : 0 : s->r.top = min((unsigned int)max(s->r.top, 0),
1049 : : node->q_data.height - MIN_DIM);
1050 : 0 : s->r.width = max(min(s->r.width,
1051 : : node->q_data.width - s->r.left), MIN_DIM);
1052 : 0 : s->r.height = max(min(s->r.height,
1053 : : node->q_data.height - s->r.top), MIN_DIM);
1054 : 0 : break;
1055 : : case V4L2_SEL_TGT_CROP_DEFAULT:
1056 : : /* Default (i.e. no) crop window. */
1057 : 0 : s->r.left = 0;
1058 : 0 : s->r.top = 0;
1059 : 0 : s->r.width = node->q_data.width;
1060 : 0 : s->r.height = node->q_data.height;
1061 : 0 : break;
1062 : : default:
1063 : : return -EINVAL;
1064 : : }
1065 : :
1066 : 0 : crop.rect.x = s->r.left;
1067 : 0 : crop.rect.y = s->r.top;
1068 : 0 : crop.rect.width = s->r.width;
1069 : 0 : crop.rect.height = s->r.height;
1070 : :
1071 : 0 : return vchiq_mmal_port_parameter_set(dev->mmal_instance, port,
1072 : : MMAL_PARAMETER_CROP,
1073 : : &crop, sizeof(crop));
1074 : : }
1075 : :
1076 : 0 : static int bcm2835_isp_node_g_selection(struct file *file, void *fh,
1077 : : struct v4l2_selection *s)
1078 : : {
1079 : : struct mmal_parameter_crop crop;
1080 : : struct bcm2835_isp_node *node = video_drvdata(file);
1081 : 0 : struct vchiq_mmal_port *port = get_port_data(node);
1082 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
1083 : 0 : u32 crop_size = sizeof(crop);
1084 : : int ret;
1085 : :
1086 : : /* We can only return out an input crop. */
1087 : 0 : switch (s->target) {
1088 : : case V4L2_SEL_TGT_CROP:
1089 : 0 : ret = vchiq_mmal_port_parameter_get(dev->mmal_instance, port,
1090 : : MMAL_PARAMETER_CROP,
1091 : : &crop, &crop_size);
1092 : 0 : if (!ret) {
1093 : 0 : s->r.left = crop.rect.x;
1094 : 0 : s->r.top = crop.rect.y;
1095 : 0 : s->r.width = crop.rect.width;
1096 : 0 : s->r.height = crop.rect.height;
1097 : : }
1098 : : break;
1099 : : case V4L2_SEL_TGT_CROP_DEFAULT:
1100 : : case V4L2_SEL_TGT_CROP_BOUNDS:
1101 : : /* Default (i.e. no) crop window. */
1102 : 0 : s->r.left = 0;
1103 : 0 : s->r.top = 0;
1104 : 0 : s->r.width = node->q_data.width;
1105 : 0 : s->r.height = node->q_data.height;
1106 : : ret = 0;
1107 : 0 : break;
1108 : : default:
1109 : : ret = -EINVAL;
1110 : : }
1111 : :
1112 : 0 : return ret;
1113 : : }
1114 : :
1115 : 0 : static int bcm3285_isp_subscribe_event(struct v4l2_fh *fh,
1116 : : const struct v4l2_event_subscription *s)
1117 : : {
1118 : 0 : switch (s->type) {
1119 : : /* Cannot change source parameters dynamically at runtime. */
1120 : : case V4L2_EVENT_SOURCE_CHANGE:
1121 : : return -EINVAL;
1122 : : case V4L2_EVENT_CTRL:
1123 : 0 : return v4l2_ctrl_subscribe_event(fh, s);
1124 : : default:
1125 : 0 : return v4l2_event_subscribe(fh, s, 4, NULL);
1126 : : }
1127 : : }
1128 : :
1129 : : static const struct v4l2_ioctl_ops bcm2835_isp_node_ioctl_ops = {
1130 : : .vidioc_querycap = bcm2835_isp_node_querycap,
1131 : : .vidioc_g_fmt_vid_cap = bcm2835_isp_node_g_fmt,
1132 : : .vidioc_g_fmt_vid_out = bcm2835_isp_node_g_fmt,
1133 : : .vidioc_g_fmt_meta_cap = bcm2835_isp_node_g_fmt,
1134 : : .vidioc_s_fmt_vid_cap = bcm2835_isp_node_s_fmt,
1135 : : .vidioc_s_fmt_vid_out = bcm2835_isp_node_s_fmt,
1136 : : .vidioc_s_fmt_meta_cap = bcm2835_isp_node_s_fmt,
1137 : : .vidioc_try_fmt_vid_cap = bcm2835_isp_node_try_fmt,
1138 : : .vidioc_try_fmt_vid_out = bcm2835_isp_node_try_fmt,
1139 : : .vidioc_try_fmt_meta_cap = bcm2835_isp_node_try_fmt,
1140 : : .vidioc_s_selection = bcm2835_isp_node_s_selection,
1141 : : .vidioc_g_selection = bcm2835_isp_node_g_selection,
1142 : :
1143 : : .vidioc_enum_fmt_vid_cap = bcm2835_isp_node_enum_fmt,
1144 : : .vidioc_enum_fmt_vid_out = bcm2835_isp_node_enum_fmt,
1145 : : .vidioc_enum_fmt_meta_cap = bcm2835_isp_node_enum_fmt,
1146 : : .vidioc_enum_framesizes = bcm2835_isp_enum_framesizes,
1147 : :
1148 : : .vidioc_reqbufs = vb2_ioctl_reqbufs,
1149 : : .vidioc_querybuf = vb2_ioctl_querybuf,
1150 : : .vidioc_qbuf = vb2_ioctl_qbuf,
1151 : : .vidioc_dqbuf = vb2_ioctl_dqbuf,
1152 : : .vidioc_expbuf = vb2_ioctl_expbuf,
1153 : : .vidioc_create_bufs = vb2_ioctl_create_bufs,
1154 : : .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
1155 : :
1156 : : .vidioc_streamon = vb2_ioctl_streamon,
1157 : : .vidioc_streamoff = vb2_ioctl_streamoff,
1158 : :
1159 : : .vidioc_subscribe_event = bcm3285_isp_subscribe_event,
1160 : : .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1161 : : };
1162 : :
1163 : : /*
1164 : : * Size of the array to provide to the VPU when asking for the list of supported
1165 : : * formats.
1166 : : *
1167 : : * The ISP component currently advertises 44 input formats, so add a small
1168 : : * overhead on that.
1169 : : */
1170 : : #define MAX_SUPPORTED_ENCODINGS 50
1171 : :
1172 : : /* Populate node->supported_fmts with the formats supported by those ports. */
1173 : 3 : static int bcm2835_isp_get_supported_fmts(struct bcm2835_isp_node *node)
1174 : : {
1175 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
1176 : : struct bcm2835_isp_fmt const **list;
1177 : : unsigned int i, j, num_encodings;
1178 : : u32 fourccs[MAX_SUPPORTED_ENCODINGS];
1179 : 3 : u32 param_size = sizeof(fourccs);
1180 : : int ret;
1181 : :
1182 : 3 : ret = vchiq_mmal_port_parameter_get(dev->mmal_instance,
1183 : : get_port_data(node),
1184 : : MMAL_PARAMETER_SUPPORTED_ENCODINGS,
1185 : : &fourccs, ¶m_size);
1186 : :
1187 : 3 : if (ret) {
1188 : 0 : if (ret == MMAL_MSG_STATUS_ENOSPC) {
1189 : 0 : v4l2_err(&dev->v4l2_dev,
1190 : : "%s: port has more encoding than we provided space for. Some are dropped.\n",
1191 : : __func__);
1192 : : num_encodings = MAX_SUPPORTED_ENCODINGS;
1193 : : } else {
1194 : 0 : v4l2_err(&dev->v4l2_dev, "%s: get_param ret %u.\n",
1195 : : __func__, ret);
1196 : 0 : return -EINVAL;
1197 : : }
1198 : : } else {
1199 : 3 : num_encodings = param_size / sizeof(u32);
1200 : : }
1201 : :
1202 : : /*
1203 : : * Assume at this stage that all encodings will be supported in V4L2.
1204 : : * Any that aren't supported will waste a very small amount of memory.
1205 : : */
1206 : 3 : list = devm_kzalloc(dev->dev,
1207 : : sizeof(struct bcm2835_isp_fmt *) * num_encodings,
1208 : : GFP_KERNEL);
1209 : 3 : if (!list)
1210 : : return -ENOMEM;
1211 : 3 : node->supported_fmts.list = list;
1212 : :
1213 : 3 : for (i = 0, j = 0; i < num_encodings; i++) {
1214 : 3 : const struct bcm2835_isp_fmt *fmt = get_fmt(fourccs[i]);
1215 : :
1216 : 3 : if (fmt) {
1217 : 3 : list[j] = fmt;
1218 : 3 : j++;
1219 : : }
1220 : : }
1221 : 3 : node->supported_fmts.num_entries = j;
1222 : :
1223 : 3 : return 0;
1224 : : }
1225 : :
1226 : : /*
1227 : : * Register a device node /dev/video<N> to go along with one of the ISP's input
1228 : : * or output nodes.
1229 : : */
1230 : 3 : static int register_node(struct bcm2835_isp_dev *dev,
1231 : : struct bcm2835_isp_node *node,
1232 : : int index)
1233 : : {
1234 : : struct video_device *vfd;
1235 : : struct vb2_queue *queue;
1236 : : int ret;
1237 : :
1238 : 3 : mutex_init(&node->lock);
1239 : :
1240 : 3 : node->dev = dev;
1241 : 3 : vfd = &node->vfd;
1242 : 3 : queue = &node->queue;
1243 : 3 : queue->type = index_to_queue_type(index);
1244 : : /*
1245 : : * Setup the node type-specific params.
1246 : : *
1247 : : * Only the OUTPUT node can set controls and crop windows. However,
1248 : : * we must allow the s/g_selection ioctl on the stats node as v4l2
1249 : : * compliance expects it to return a -ENOTTY, and the framework
1250 : : * does not handle it if the ioctl is disabled.
1251 : : */
1252 : 3 : switch (queue->type) {
1253 : : case V4L2_BUF_TYPE_VIDEO_OUTPUT:
1254 : 3 : vfd->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
1255 : 3 : node->id = index;
1256 : 3 : node->vfl_dir = VFL_DIR_TX;
1257 : 3 : node->name = "output";
1258 : 3 : break;
1259 : : case V4L2_BUF_TYPE_VIDEO_CAPTURE:
1260 : 3 : vfd->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1261 : : /* First Capture node starts at id 0, etc. */
1262 : 3 : node->id = index - BCM2835_ISP_NUM_OUTPUTS;
1263 : 3 : node->vfl_dir = VFL_DIR_RX;
1264 : 3 : node->name = "capture";
1265 : : v4l2_disable_ioctl(&node->vfd, VIDIOC_S_CTRL);
1266 : : v4l2_disable_ioctl(&node->vfd, VIDIOC_S_SELECTION);
1267 : : v4l2_disable_ioctl(&node->vfd, VIDIOC_G_SELECTION);
1268 : : break;
1269 : : case V4L2_BUF_TYPE_META_CAPTURE:
1270 : 3 : vfd->device_caps = V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING;
1271 : 3 : node->id = index - BCM2835_ISP_NUM_OUTPUTS;
1272 : 3 : node->vfl_dir = VFL_DIR_RX;
1273 : 3 : node->name = "stats";
1274 : : v4l2_disable_ioctl(&node->vfd, VIDIOC_S_CTRL);
1275 : : v4l2_disable_ioctl(&node->vfd, VIDIOC_S_SELECTION);
1276 : : v4l2_disable_ioctl(&node->vfd, VIDIOC_G_SELECTION);
1277 : : break;
1278 : : }
1279 : :
1280 : : /* We use the selection API instead of the old crop API. */
1281 : : v4l2_disable_ioctl(vfd, VIDIOC_CROPCAP);
1282 : : v4l2_disable_ioctl(vfd, VIDIOC_G_CROP);
1283 : : v4l2_disable_ioctl(vfd, VIDIOC_S_CROP);
1284 : :
1285 : 3 : ret = bcm2835_isp_get_supported_fmts(node);
1286 : 3 : if (ret)
1287 : : return ret;
1288 : :
1289 : : /* Initialise the the video node. */
1290 : 3 : vfd->vfl_type = VFL_TYPE_GRABBER;
1291 : 3 : vfd->fops = &bcm2835_isp_fops,
1292 : 3 : vfd->ioctl_ops = &bcm2835_isp_node_ioctl_ops,
1293 : 3 : vfd->minor = -1,
1294 : 3 : vfd->release = video_device_release_empty,
1295 : 3 : vfd->queue = &node->queue;
1296 : 3 : vfd->lock = &node->lock;
1297 : 3 : vfd->v4l2_dev = &dev->v4l2_dev;
1298 : 3 : vfd->vfl_dir = node->vfl_dir;
1299 : :
1300 : 3 : node->q_data.fmt = get_default_format(node);
1301 : 3 : node->q_data.width = DEFAULT_DIM;
1302 : 3 : node->q_data.height = DEFAULT_DIM;
1303 : 3 : node->q_data.bytesperline =
1304 : : get_bytesperline(DEFAULT_DIM, node->q_data.fmt);
1305 : 3 : node->q_data.sizeimage = node_is_stats(node) ?
1306 : 3 : get_port_data(node)->recommended_buffer.size :
1307 : 3 : get_sizeimage(node->q_data.bytesperline,
1308 : : node->q_data.width,
1309 : : node->q_data.height,
1310 : : node->q_data.fmt);
1311 : :
1312 : 3 : queue->io_modes = VB2_MMAP | VB2_DMABUF;
1313 : 3 : queue->drv_priv = node;
1314 : 3 : queue->ops = &bcm2835_isp_node_queue_ops;
1315 : 3 : queue->mem_ops = &vb2_dma_contig_memops;
1316 : 3 : queue->buf_struct_size = sizeof(struct bcm2835_isp_buffer);
1317 : 3 : queue->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1318 : 3 : queue->dev = dev->dev;
1319 : 3 : queue->lock = &node->queue_lock;
1320 : :
1321 : 3 : ret = vb2_queue_init(queue);
1322 : 3 : if (ret < 0) {
1323 : 0 : v4l2_info(&dev->v4l2_dev, "vb2_queue_init failed\n");
1324 : 0 : return ret;
1325 : : }
1326 : 3 : node->queue_init = true;
1327 : :
1328 : : /* Set some controls and defaults, but only on the VIDEO_OUTPUT node. */
1329 : 3 : if (node_is_output(node)) {
1330 : : unsigned int i;
1331 : :
1332 : : /* Use this ctrl template to assign all out ISP custom ctrls. */
1333 : 3 : struct v4l2_ctrl_config ctrl_template = {
1334 : : .ops = &bcm2835_isp_ctrl_ops,
1335 : : .type = V4L2_CTRL_TYPE_U8,
1336 : : .def = 0,
1337 : : .min = 0x00,
1338 : : .max = 0xff,
1339 : : .step = 1,
1340 : : };
1341 : :
1342 : 3 : ret = v4l2_ctrl_handler_init(&dev->ctrl_handler, 12);
1343 : 3 : if (ret) {
1344 : 0 : v4l2_err(&dev->v4l2_dev, "ctrl_handler init failed (%d)\n",
1345 : : ret);
1346 : 0 : return ret;
1347 : : }
1348 : :
1349 : 3 : dev->r_gain = 1000;
1350 : 3 : dev->b_gain = 1000;
1351 : :
1352 : 3 : v4l2_ctrl_new_std(&dev->ctrl_handler, &bcm2835_isp_ctrl_ops,
1353 : : V4L2_CID_RED_BALANCE, 1, 0xffff, 1,
1354 : : dev->r_gain);
1355 : :
1356 : 3 : v4l2_ctrl_new_std(&dev->ctrl_handler, &bcm2835_isp_ctrl_ops,
1357 : : V4L2_CID_BLUE_BALANCE, 1, 0xffff, 1,
1358 : 3 : dev->b_gain);
1359 : :
1360 : 3 : v4l2_ctrl_new_std(&dev->ctrl_handler, &bcm2835_isp_ctrl_ops,
1361 : : V4L2_CID_DIGITAL_GAIN, 1, 0xffff, 1, 1000);
1362 : :
1363 : 3 : for (i = 0; i < ARRAY_SIZE(custom_ctrls); i++) {
1364 : 3 : ctrl_template.name = custom_ctrls[i].name;
1365 : 3 : ctrl_template.id = custom_ctrls[i].id;
1366 : 3 : ctrl_template.dims[0] = custom_ctrls[i].size;
1367 : 3 : ctrl_template.flags = custom_ctrls[i].flags;
1368 : 3 : v4l2_ctrl_new_custom(&dev->ctrl_handler,
1369 : : &ctrl_template, NULL);
1370 : : }
1371 : :
1372 : 3 : node->vfd.ctrl_handler = &dev->ctrl_handler;
1373 : 3 : if (dev->ctrl_handler.error) {
1374 : : ret = dev->ctrl_handler.error;
1375 : 0 : v4l2_err(&dev->v4l2_dev, "controls init failed (%d)\n",
1376 : : ret);
1377 : 0 : v4l2_ctrl_handler_free(&dev->ctrl_handler);
1378 : 0 : goto ctrl_cleanup;
1379 : : }
1380 : : }
1381 : :
1382 : : /* Define the device names */
1383 : 3 : snprintf(vfd->name, sizeof(node->vfd.name), "%s-%s%d", BCM2835_ISP_NAME,
1384 : : node->name, node->id);
1385 : :
1386 : 3 : ret = video_register_device(vfd, VFL_TYPE_GRABBER, video_nr + index);
1387 : 3 : if (ret) {
1388 : 0 : v4l2_err(&dev->v4l2_dev,
1389 : : "Failed to register video %s[%d] device node\n",
1390 : : node->name, node->id);
1391 : 0 : goto ctrl_cleanup;
1392 : : }
1393 : :
1394 : 3 : node->registered = true;
1395 : : video_set_drvdata(vfd, node);
1396 : :
1397 : 3 : v4l2_info(&dev->v4l2_dev,
1398 : : "Device node %s[%d] registered as /dev/video%d\n",
1399 : : node->name, node->id, vfd->num);
1400 : :
1401 : 3 : return 0;
1402 : :
1403 : : ctrl_cleanup:
1404 : 0 : v4l2_ctrl_handler_free(&dev->ctrl_handler);
1405 : 0 : return ret;
1406 : : }
1407 : :
1408 : : /* Unregister one of the /dev/video<N> nodes associated with the ISP. */
1409 : 0 : static void unregister_node(struct bcm2835_isp_node *node)
1410 : : {
1411 : : struct bcm2835_isp_dev *dev = node_get_dev(node);
1412 : :
1413 : 0 : v4l2_info(&dev->v4l2_dev,
1414 : : "Unregistering node %s[%d] device node /dev/video%d\n",
1415 : : node->name, node->id, node->vfd.num);
1416 : :
1417 : 0 : if (node->queue_init)
1418 : 0 : vb2_queue_release(&node->queue);
1419 : :
1420 : 0 : if (node->registered) {
1421 : 0 : video_unregister_device(&node->vfd);
1422 : 0 : if (node_is_output(node))
1423 : 0 : v4l2_ctrl_handler_free(&dev->ctrl_handler);
1424 : : }
1425 : :
1426 : : /*
1427 : : * node->supported_fmts.list is free'd automatically
1428 : : * as a managed resource.
1429 : : */
1430 : 0 : node->supported_fmts.list = NULL;
1431 : 0 : node->supported_fmts.num_entries = 0;
1432 : 0 : node->vfd.ctrl_handler = NULL;
1433 : 0 : node->registered = false;
1434 : 0 : node->queue_init = false;
1435 : 0 : }
1436 : :
1437 : 0 : static void media_controller_unregister(struct bcm2835_isp_dev *dev)
1438 : : {
1439 : : unsigned int i;
1440 : :
1441 : 0 : v4l2_info(&dev->v4l2_dev, "Unregister from media controller\n");
1442 : :
1443 : 0 : if (dev->media_device_registered) {
1444 : 0 : media_device_unregister(&dev->mdev);
1445 : 0 : media_device_cleanup(&dev->mdev);
1446 : 0 : dev->media_device_registered = false;
1447 : : }
1448 : :
1449 : 0 : kfree(dev->entity.name);
1450 : 0 : dev->entity.name = NULL;
1451 : :
1452 : 0 : if (dev->media_entity_registered) {
1453 : 0 : media_device_unregister_entity(&dev->entity);
1454 : 0 : dev->media_entity_registered = false;
1455 : : }
1456 : :
1457 : 0 : for (i = 0; i < BCM2835_ISP_NUM_NODES; i++) {
1458 : : struct bcm2835_isp_node *node = &dev->node[i];
1459 : :
1460 : 0 : if (node->media_node_registered) {
1461 : 0 : media_remove_intf_links(node->intf_link->intf);
1462 : 0 : media_entity_remove_links(&dev->node[i].vfd.entity);
1463 : 0 : media_devnode_remove(node->intf_devnode);
1464 : 0 : media_device_unregister_entity(&node->vfd.entity);
1465 : 0 : kfree(node->vfd.entity.name);
1466 : : }
1467 : 0 : node->media_node_registered = false;
1468 : : }
1469 : :
1470 : 0 : dev->v4l2_dev.mdev = NULL;
1471 : 0 : }
1472 : :
1473 : 3 : static int media_controller_register_node(struct bcm2835_isp_dev *dev, int num)
1474 : : {
1475 : : struct bcm2835_isp_node *node = &dev->node[num];
1476 : 3 : struct media_entity *entity = &node->vfd.entity;
1477 : 3 : int output = node_is_output(node);
1478 : : char *name;
1479 : : int ret;
1480 : :
1481 : 3 : v4l2_info(&dev->v4l2_dev,
1482 : : "Register %s node %d with media controller\n",
1483 : : output ? "output" : "capture", num);
1484 : 3 : entity->obj_type = MEDIA_ENTITY_TYPE_VIDEO_DEVICE;
1485 : 3 : entity->function = MEDIA_ENT_F_IO_V4L;
1486 : 3 : entity->info.dev.major = VIDEO_MAJOR;
1487 : 3 : entity->info.dev.minor = node->vfd.minor;
1488 : : name = kmalloc(BCM2835_ISP_ENTITY_NAME_LEN, GFP_KERNEL);
1489 : 3 : if (!name) {
1490 : : ret = -ENOMEM;
1491 : : goto error_no_mem;
1492 : : }
1493 : 3 : snprintf(name, BCM2835_ISP_ENTITY_NAME_LEN, "%s0-%s%d",
1494 : : BCM2835_ISP_NAME, output ? "output" : "capture", num);
1495 : 3 : entity->name = name;
1496 : 3 : node->pad.flags = output ? MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
1497 : 3 : ret = media_entity_pads_init(entity, 1, &node->pad);
1498 : 3 : if (ret)
1499 : : goto error_pads_init;
1500 : 3 : ret = media_device_register_entity(&dev->mdev, entity);
1501 : 3 : if (ret)
1502 : : goto error_register_entity;
1503 : :
1504 : 3 : node->intf_devnode = media_devnode_create(&dev->mdev,
1505 : : MEDIA_INTF_T_V4L_VIDEO, 0,
1506 : 3 : VIDEO_MAJOR, node->vfd.minor);
1507 : 3 : if (!node->intf_devnode) {
1508 : : ret = -ENOMEM;
1509 : : goto error_devnode_create;
1510 : : }
1511 : :
1512 : 3 : node->intf_link = media_create_intf_link(entity,
1513 : : &node->intf_devnode->intf,
1514 : : MEDIA_LNK_FL_IMMUTABLE |
1515 : : MEDIA_LNK_FL_ENABLED);
1516 : 3 : if (!node->intf_link) {
1517 : : ret = -ENOMEM;
1518 : : goto error_create_intf_link;
1519 : : }
1520 : :
1521 : 3 : if (output)
1522 : 3 : ret = media_create_pad_link(entity, 0, &dev->entity, num,
1523 : : MEDIA_LNK_FL_IMMUTABLE |
1524 : : MEDIA_LNK_FL_ENABLED);
1525 : : else
1526 : 3 : ret = media_create_pad_link(&dev->entity, num, entity, 0,
1527 : : MEDIA_LNK_FL_IMMUTABLE |
1528 : : MEDIA_LNK_FL_ENABLED);
1529 : 3 : if (ret)
1530 : : goto error_create_pad_link;
1531 : :
1532 : 3 : dev->node[num].media_node_registered = true;
1533 : 3 : return 0;
1534 : :
1535 : : error_create_pad_link:
1536 : 0 : media_remove_intf_links(&node->intf_devnode->intf);
1537 : : error_create_intf_link:
1538 : 0 : media_devnode_remove(node->intf_devnode);
1539 : : error_devnode_create:
1540 : 0 : media_device_unregister_entity(&node->vfd.entity);
1541 : : error_register_entity:
1542 : : error_pads_init:
1543 : 0 : kfree(entity->name);
1544 : 0 : entity->name = NULL;
1545 : : error_no_mem:
1546 : 0 : if (ret)
1547 : 0 : v4l2_info(&dev->v4l2_dev, "Error registering node\n");
1548 : :
1549 : 0 : return ret;
1550 : : }
1551 : :
1552 : 3 : static int media_controller_register(struct bcm2835_isp_dev *dev)
1553 : : {
1554 : : char *name;
1555 : : unsigned int i;
1556 : : int ret;
1557 : :
1558 : 3 : v4l2_dbg(2, debug, &dev->v4l2_dev, "Registering with media controller\n");
1559 : 3 : dev->mdev.dev = dev->dev;
1560 : 3 : strscpy(dev->mdev.model, "bcm2835-isp",
1561 : : sizeof(dev->mdev.model));
1562 : 3 : strscpy(dev->mdev.bus_info, "platform:bcm2835-isp",
1563 : : sizeof(dev->mdev.bus_info));
1564 : 3 : media_device_init(&dev->mdev);
1565 : 3 : dev->v4l2_dev.mdev = &dev->mdev;
1566 : :
1567 : 3 : v4l2_dbg(2, debug, &dev->v4l2_dev, "Register entity for nodes\n");
1568 : :
1569 : : name = kmalloc(BCM2835_ISP_ENTITY_NAME_LEN, GFP_KERNEL);
1570 : 3 : if (!name) {
1571 : : ret = -ENOMEM;
1572 : : goto done;
1573 : : }
1574 : 3 : snprintf(name, BCM2835_ISP_ENTITY_NAME_LEN, "bcm2835_isp0");
1575 : 3 : dev->entity.name = name;
1576 : 3 : dev->entity.obj_type = MEDIA_ENTITY_TYPE_BASE;
1577 : 3 : dev->entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
1578 : :
1579 : 3 : for (i = 0; i < BCM2835_ISP_NUM_NODES; i++) {
1580 : 3 : dev->pad[i].flags = node_is_output(&dev->node[i]) ?
1581 : : MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
1582 : : }
1583 : :
1584 : 3 : ret = media_entity_pads_init(&dev->entity, BCM2835_ISP_NUM_NODES,
1585 : 3 : dev->pad);
1586 : 3 : if (ret)
1587 : : goto done;
1588 : :
1589 : 3 : ret = media_device_register_entity(&dev->mdev, &dev->entity);
1590 : 3 : if (ret)
1591 : : goto done;
1592 : :
1593 : 3 : dev->media_entity_registered = true;
1594 : 3 : for (i = 0; i < BCM2835_ISP_NUM_NODES; i++) {
1595 : 3 : ret = media_controller_register_node(dev, i);
1596 : 3 : if (ret)
1597 : : goto done;
1598 : : }
1599 : :
1600 : 3 : ret = media_device_register(&dev->mdev);
1601 : 3 : if (!ret)
1602 : 3 : dev->media_device_registered = true;
1603 : : done:
1604 : 3 : return ret;
1605 : : }
1606 : :
1607 : 0 : static int bcm2835_isp_remove(struct platform_device *pdev)
1608 : : {
1609 : : struct bcm2835_isp_dev *dev = platform_get_drvdata(pdev);
1610 : : unsigned int i;
1611 : :
1612 : 0 : media_controller_unregister(dev);
1613 : :
1614 : 0 : for (i = 0; i < BCM2835_ISP_NUM_NODES; i++)
1615 : 0 : unregister_node(&dev->node[i]);
1616 : :
1617 : 0 : v4l2_device_unregister(&dev->v4l2_dev);
1618 : :
1619 : 0 : if (dev->component)
1620 : 0 : vchiq_mmal_component_finalise(dev->mmal_instance,
1621 : : dev->component);
1622 : :
1623 : 0 : vchiq_mmal_finalise(dev->mmal_instance);
1624 : :
1625 : 0 : return 0;
1626 : : }
1627 : :
1628 : 3 : static int bcm2835_isp_probe(struct platform_device *pdev)
1629 : : {
1630 : : struct bcm2835_isp_dev *dev;
1631 : : unsigned int i;
1632 : : int ret;
1633 : :
1634 : 3 : dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
1635 : 3 : if (!dev)
1636 : : return -ENOMEM;
1637 : :
1638 : 3 : dev->dev = &pdev->dev;
1639 : :
1640 : 3 : ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
1641 : 3 : if (ret)
1642 : : return ret;
1643 : :
1644 : 3 : ret = vchiq_mmal_init(&dev->mmal_instance);
1645 : 3 : if (ret) {
1646 : 0 : v4l2_device_unregister(&dev->v4l2_dev);
1647 : 0 : return ret;
1648 : : }
1649 : :
1650 : 3 : ret = vchiq_mmal_component_init(dev->mmal_instance, "ril.isp",
1651 : : &dev->component);
1652 : 3 : if (ret) {
1653 : 0 : v4l2_err(&dev->v4l2_dev,
1654 : : "%s: failed to create ril.isp component\n", __func__);
1655 : 0 : goto error;
1656 : : }
1657 : :
1658 : 3 : if ((dev->component->inputs != BCM2835_ISP_NUM_OUTPUTS) ||
1659 : 3 : (dev->component->outputs != BCM2835_ISP_NUM_CAPTURES +
1660 : : BCM2835_ISP_NUM_METADATA)) {
1661 : 0 : v4l2_err(&dev->v4l2_dev,
1662 : : "%s: ril.isp returned %d i/p (%d expected), %d o/p (%d expected) ports\n",
1663 : : __func__, dev->component->inputs,
1664 : : BCM2835_ISP_NUM_OUTPUTS,
1665 : : dev->component->outputs,
1666 : : BCM2835_ISP_NUM_CAPTURES + BCM2835_ISP_NUM_METADATA);
1667 : 0 : goto error;
1668 : : }
1669 : :
1670 : : atomic_set(&dev->num_streaming, 0);
1671 : :
1672 : 3 : for (i = 0; i < BCM2835_ISP_NUM_NODES; i++) {
1673 : 3 : struct bcm2835_isp_node *node = &dev->node[i];
1674 : :
1675 : 3 : ret = register_node(dev, node, i);
1676 : 3 : if (ret)
1677 : : goto error;
1678 : : }
1679 : :
1680 : 3 : ret = media_controller_register(dev);
1681 : 3 : if (ret)
1682 : : goto error;
1683 : :
1684 : : platform_set_drvdata(pdev, dev);
1685 : 3 : v4l2_info(&dev->v4l2_dev, "Loaded V4L2 %s\n", BCM2835_ISP_NAME);
1686 : 3 : return 0;
1687 : :
1688 : : error:
1689 : 0 : bcm2835_isp_remove(pdev);
1690 : :
1691 : 0 : return ret;
1692 : : }
1693 : :
1694 : : static struct platform_driver bcm2835_isp_pdrv = {
1695 : : .probe = bcm2835_isp_probe,
1696 : : .remove = bcm2835_isp_remove,
1697 : : .driver = {
1698 : : .name = BCM2835_ISP_NAME,
1699 : : },
1700 : : };
1701 : :
1702 : 3 : module_platform_driver(bcm2835_isp_pdrv);
1703 : :
1704 : : MODULE_DESCRIPTION("BCM2835 ISP driver");
1705 : : MODULE_AUTHOR("Naushir Patuck <naush@raspberrypi.com>");
1706 : : MODULE_LICENSE("GPL");
1707 : : MODULE_VERSION("1.0");
1708 : : MODULE_ALIAS("platform:bcm2835-isp");
|
