1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Broadcom BCM2835 V4L2 driver 4 * 5 * Copyright © 2013 Raspberry Pi (Trading) Ltd. 6 * 7 * Authors: Vincent Sanders @ Collabora 8 * Dave Stevenson @ Broadcom 9 * (now dave.stevenson@raspberrypi.org) 10 * Simon Mellor @ Broadcom 11 * Luke Diamand @ Broadcom 12 */ 13 14 #include <linux/dma-mapping.h> 15 #include <linux/errno.h> 16 #include <linux/kernel.h> 17 #include <linux/module.h> 18 #include <linux/slab.h> 19 #include <media/videobuf2-vmalloc.h> 20 #include <media/videobuf2-dma-contig.h> 21 #include <media/v4l2-device.h> 22 #include <media/v4l2-ioctl.h> 23 #include <media/v4l2-ctrls.h> 24 #include <media/v4l2-fh.h> 25 #include <media/v4l2-event.h> 26 #include <media/v4l2-common.h> 27 #include <linux/delay.h> 28 29 #include "../interface/vchiq_arm/vchiq_bus.h" 30 #include "../vchiq-mmal/mmal-common.h" 31 #include "../vchiq-mmal/mmal-encodings.h" 32 #include "../vchiq-mmal/mmal-vchiq.h" 33 #include "../vchiq-mmal/mmal-msg.h" 34 #include "../vchiq-mmal/mmal-parameters.h" 35 #include "bcm2835-camera.h" 36 37 #define MIN_WIDTH 32 38 #define MIN_HEIGHT 32 39 #define MIN_BUFFER_SIZE (80 * 1024) 40 41 #define MAX_VIDEO_MODE_WIDTH 1280 42 #define MAX_VIDEO_MODE_HEIGHT 720 43 44 #define MAX_BCM2835_CAMERAS 2 45 46 int bcm2835_v4l2_debug; 47 module_param_named(debug, bcm2835_v4l2_debug, int, 0644); 48 MODULE_PARM_DESC(bcm2835_v4l2_debug, "Debug level 0-2"); 49 50 #define UNSET (-1) 51 static int video_nr[] = {[0 ... (MAX_BCM2835_CAMERAS - 1)] = UNSET }; 52 module_param_array(video_nr, int, NULL, 0644); 53 MODULE_PARM_DESC(video_nr, "videoX start numbers, -1 is autodetect"); 54 55 static int max_video_width = MAX_VIDEO_MODE_WIDTH; 56 static int max_video_height = MAX_VIDEO_MODE_HEIGHT; 57 module_param(max_video_width, int, 0644); 58 MODULE_PARM_DESC(max_video_width, "Threshold for video mode"); 59 module_param(max_video_height, int, 0644); 60 MODULE_PARM_DESC(max_video_height, "Threshold for video mode"); 61 62 /* camera instance counter */ 63 static atomic_t camera_instance = ATOMIC_INIT(0); 64 65 /* global device data array */ 66 static struct bcm2835_mmal_dev *gdev[MAX_BCM2835_CAMERAS]; 67 68 #define FPS_MIN 1 69 #define FPS_MAX 90 70 71 /* timeperframe: min/max and default */ 72 static const struct v4l2_fract 73 tpf_min = {.numerator = 1, .denominator = FPS_MAX}, 74 tpf_max = {.numerator = 1, .denominator = FPS_MIN}, 75 tpf_default = {.numerator = 1000, .denominator = 30000}; 76 77 /* Container for MMAL and VB2 buffers*/ 78 struct vb2_mmal_buffer { 79 struct vb2_v4l2_buffer vb; 80 struct mmal_buffer mmal; 81 }; 82 83 /* video formats */ 84 static struct mmal_fmt formats[] = { 85 { 86 .fourcc = V4L2_PIX_FMT_YUV420, 87 .mmal = MMAL_ENCODING_I420, 88 .depth = 12, 89 .mmal_component = COMP_CAMERA, 90 .ybbp = 1, 91 .remove_padding = true, 92 }, { 93 .fourcc = V4L2_PIX_FMT_YUYV, 94 .mmal = MMAL_ENCODING_YUYV, 95 .depth = 16, 96 .mmal_component = COMP_CAMERA, 97 .ybbp = 2, 98 .remove_padding = false, 99 }, { 100 .fourcc = V4L2_PIX_FMT_RGB24, 101 .mmal = MMAL_ENCODING_RGB24, 102 .depth = 24, 103 .mmal_component = COMP_CAMERA, 104 .ybbp = 3, 105 .remove_padding = false, 106 }, { 107 .fourcc = V4L2_PIX_FMT_JPEG, 108 .flags = V4L2_FMT_FLAG_COMPRESSED, 109 .mmal = MMAL_ENCODING_JPEG, 110 .depth = 8, 111 .mmal_component = COMP_IMAGE_ENCODE, 112 .ybbp = 0, 113 .remove_padding = false, 114 }, { 115 .fourcc = V4L2_PIX_FMT_H264, 116 .flags = V4L2_FMT_FLAG_COMPRESSED, 117 .mmal = MMAL_ENCODING_H264, 118 .depth = 8, 119 .mmal_component = COMP_VIDEO_ENCODE, 120 .ybbp = 0, 121 .remove_padding = false, 122 }, { 123 .fourcc = V4L2_PIX_FMT_MJPEG, 124 .flags = V4L2_FMT_FLAG_COMPRESSED, 125 .mmal = MMAL_ENCODING_MJPEG, 126 .depth = 8, 127 .mmal_component = COMP_VIDEO_ENCODE, 128 .ybbp = 0, 129 .remove_padding = false, 130 }, { 131 .fourcc = V4L2_PIX_FMT_YVYU, 132 .mmal = MMAL_ENCODING_YVYU, 133 .depth = 16, 134 .mmal_component = COMP_CAMERA, 135 .ybbp = 2, 136 .remove_padding = false, 137 }, { 138 .fourcc = V4L2_PIX_FMT_VYUY, 139 .mmal = MMAL_ENCODING_VYUY, 140 .depth = 16, 141 .mmal_component = COMP_CAMERA, 142 .ybbp = 2, 143 .remove_padding = false, 144 }, { 145 .fourcc = V4L2_PIX_FMT_UYVY, 146 .mmal = MMAL_ENCODING_UYVY, 147 .depth = 16, 148 .mmal_component = COMP_CAMERA, 149 .ybbp = 2, 150 .remove_padding = false, 151 }, { 152 .fourcc = V4L2_PIX_FMT_NV12, 153 .mmal = MMAL_ENCODING_NV12, 154 .depth = 12, 155 .mmal_component = COMP_CAMERA, 156 .ybbp = 1, 157 .remove_padding = true, 158 }, { 159 .fourcc = V4L2_PIX_FMT_BGR24, 160 .mmal = MMAL_ENCODING_BGR24, 161 .depth = 24, 162 .mmal_component = COMP_CAMERA, 163 .ybbp = 3, 164 .remove_padding = false, 165 }, { 166 .fourcc = V4L2_PIX_FMT_YVU420, 167 .mmal = MMAL_ENCODING_YV12, 168 .depth = 12, 169 .mmal_component = COMP_CAMERA, 170 .ybbp = 1, 171 .remove_padding = true, 172 }, { 173 .fourcc = V4L2_PIX_FMT_NV21, 174 .mmal = MMAL_ENCODING_NV21, 175 .depth = 12, 176 .mmal_component = COMP_CAMERA, 177 .ybbp = 1, 178 .remove_padding = true, 179 }, { 180 .fourcc = V4L2_PIX_FMT_BGR32, 181 .mmal = MMAL_ENCODING_BGRA, 182 .depth = 32, 183 .mmal_component = COMP_CAMERA, 184 .ybbp = 4, 185 .remove_padding = false, 186 }, 187 }; 188 189 static struct mmal_fmt *get_format(struct v4l2_format *f) 190 { 191 struct mmal_fmt *fmt; 192 unsigned int k; 193 194 for (k = 0; k < ARRAY_SIZE(formats); k++) { 195 fmt = &formats[k]; 196 if (fmt->fourcc == f->fmt.pix.pixelformat) 197 return fmt; 198 } 199 200 return NULL; 201 } 202 203 /* ------------------------------------------------------------------ 204 * Videobuf queue operations 205 * ------------------------------------------------------------------ 206 */ 207 208 static int queue_setup(struct vb2_queue *vq, 209 unsigned int *nbuffers, unsigned int *nplanes, 210 unsigned int sizes[], struct device *alloc_ctxs[]) 211 { 212 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq); 213 unsigned long size; 214 215 /* refuse queue setup if port is not configured */ 216 if (!dev->capture.port) { 217 v4l2_err(&dev->v4l2_dev, 218 "%s: capture port not configured\n", __func__); 219 return -EINVAL; 220 } 221 222 /* Handle CREATE_BUFS situation - *nplanes != 0 */ 223 if (*nplanes) { 224 if (*nplanes != 1 || 225 sizes[0] < dev->capture.port->current_buffer.size) { 226 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 227 "%s: dev:%p Invalid buffer request from CREATE_BUFS, size %u < %u, nplanes %u != 1\n", 228 __func__, dev, sizes[0], 229 dev->capture.port->current_buffer.size, 230 *nplanes); 231 return -EINVAL; 232 } else { 233 return 0; 234 } 235 } 236 237 /* Handle REQBUFS situation */ 238 size = dev->capture.port->current_buffer.size; 239 if (size == 0) { 240 v4l2_err(&dev->v4l2_dev, 241 "%s: capture port buffer size is zero\n", __func__); 242 return -EINVAL; 243 } 244 245 if (*nbuffers < dev->capture.port->minimum_buffer.num) 246 *nbuffers = dev->capture.port->minimum_buffer.num; 247 248 dev->capture.port->current_buffer.num = *nbuffers; 249 250 *nplanes = 1; 251 252 sizes[0] = size; 253 254 /* 255 * videobuf2-vmalloc allocator is context-less so no need to set 256 * alloc_ctxs array. 257 */ 258 259 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n", 260 __func__, dev); 261 262 return 0; 263 } 264 265 static int buffer_init(struct vb2_buffer *vb) 266 { 267 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue); 268 struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb); 269 struct vb2_mmal_buffer *buf = 270 container_of(vb2, struct vb2_mmal_buffer, vb); 271 272 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n", 273 __func__, dev, vb); 274 buf->mmal.buffer = vb2_plane_vaddr(&buf->vb.vb2_buf, 0); 275 buf->mmal.buffer_size = vb2_plane_size(&buf->vb.vb2_buf, 0); 276 277 return mmal_vchi_buffer_init(dev->instance, &buf->mmal); 278 } 279 280 static int buffer_prepare(struct vb2_buffer *vb) 281 { 282 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue); 283 unsigned long size; 284 285 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n", 286 __func__, dev, vb); 287 288 if (!dev->capture.port || !dev->capture.fmt) 289 return -ENODEV; 290 291 size = dev->capture.stride * dev->capture.height; 292 if (vb2_plane_size(vb, 0) < size) { 293 v4l2_err(&dev->v4l2_dev, 294 "%s data will not fit into plane (%lu < %lu)\n", 295 __func__, vb2_plane_size(vb, 0), size); 296 return -EINVAL; 297 } 298 299 return 0; 300 } 301 302 static void buffer_cleanup(struct vb2_buffer *vb) 303 { 304 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue); 305 struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb); 306 struct vb2_mmal_buffer *buf = 307 container_of(vb2, struct vb2_mmal_buffer, vb); 308 309 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n", 310 __func__, dev, vb); 311 312 mmal_vchi_buffer_cleanup(&buf->mmal); 313 } 314 315 static inline bool is_capturing(struct bcm2835_mmal_dev *dev) 316 { 317 return dev->capture.camera_port == 318 &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE]; 319 } 320 321 static void buffer_cb(struct vchiq_mmal_instance *instance, 322 struct vchiq_mmal_port *port, 323 int status, 324 struct mmal_buffer *mmal_buf) 325 { 326 struct bcm2835_mmal_dev *dev = port->cb_ctx; 327 struct vb2_mmal_buffer *buf = 328 container_of(mmal_buf, struct vb2_mmal_buffer, mmal); 329 330 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 331 "%s: status:%d, buf:%p, length:%lu, flags %u, pts %lld\n", 332 __func__, status, buf, mmal_buf->length, mmal_buf->mmal_flags, 333 mmal_buf->pts); 334 335 if (status) { 336 /* error in transfer */ 337 if (buf) { 338 /* there was a buffer with the error so return it */ 339 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); 340 } 341 return; 342 } 343 344 if (mmal_buf->length == 0) { 345 /* stream ended */ 346 if (dev->capture.frame_count) { 347 /* empty buffer whilst capturing - expected to be an 348 * EOS, so grab another frame 349 */ 350 if (is_capturing(dev)) { 351 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 352 "Grab another frame"); 353 vchiq_mmal_port_parameter_set( 354 instance, 355 dev->capture.camera_port, 356 MMAL_PARAMETER_CAPTURE, 357 &dev->capture.frame_count, 358 sizeof(dev->capture.frame_count)); 359 } 360 if (vchiq_mmal_submit_buffer(instance, port, 361 &buf->mmal)) 362 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 363 "Failed to return EOS buffer"); 364 } else { 365 /* stopping streaming. 366 * return buffer, and signal frame completion 367 */ 368 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); 369 complete(&dev->capture.frame_cmplt); 370 } 371 return; 372 } 373 374 if (!dev->capture.frame_count) { 375 /* signal frame completion */ 376 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); 377 complete(&dev->capture.frame_cmplt); 378 return; 379 } 380 381 if (dev->capture.vc_start_timestamp != -1 && mmal_buf->pts) { 382 ktime_t timestamp; 383 s64 runtime_us = mmal_buf->pts - 384 dev->capture.vc_start_timestamp; 385 timestamp = ktime_add_us(dev->capture.kernel_start_ts, 386 runtime_us); 387 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 388 "Convert start time %llu and %llu with offset %llu to %llu\n", 389 ktime_to_ns(dev->capture.kernel_start_ts), 390 dev->capture.vc_start_timestamp, mmal_buf->pts, 391 ktime_to_ns(timestamp)); 392 buf->vb.vb2_buf.timestamp = ktime_to_ns(timestamp); 393 } else { 394 buf->vb.vb2_buf.timestamp = ktime_get_ns(); 395 } 396 buf->vb.sequence = dev->capture.sequence++; 397 buf->vb.field = V4L2_FIELD_NONE; 398 399 vb2_set_plane_payload(&buf->vb.vb2_buf, 0, mmal_buf->length); 400 if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_KEYFRAME) 401 buf->vb.flags |= V4L2_BUF_FLAG_KEYFRAME; 402 403 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE); 404 405 if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS && 406 is_capturing(dev)) { 407 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 408 "Grab another frame as buffer has EOS"); 409 vchiq_mmal_port_parameter_set( 410 instance, 411 dev->capture.camera_port, 412 MMAL_PARAMETER_CAPTURE, 413 &dev->capture.frame_count, 414 sizeof(dev->capture.frame_count)); 415 } 416 } 417 418 static int enable_camera(struct bcm2835_mmal_dev *dev) 419 { 420 int ret; 421 422 if (!dev->camera_use_count) { 423 ret = vchiq_mmal_port_parameter_set( 424 dev->instance, 425 &dev->component[COMP_CAMERA]->control, 426 MMAL_PARAMETER_CAMERA_NUM, &dev->camera_num, 427 sizeof(dev->camera_num)); 428 if (ret < 0) { 429 v4l2_err(&dev->v4l2_dev, 430 "Failed setting camera num, ret %d\n", ret); 431 return -EINVAL; 432 } 433 434 ret = vchiq_mmal_component_enable(dev->instance, 435 dev->component[COMP_CAMERA]); 436 if (ret < 0) { 437 v4l2_err(&dev->v4l2_dev, 438 "Failed enabling camera, ret %d\n", ret); 439 return -EINVAL; 440 } 441 } 442 dev->camera_use_count++; 443 v4l2_dbg(1, bcm2835_v4l2_debug, 444 &dev->v4l2_dev, "enabled camera (refcount %d)\n", 445 dev->camera_use_count); 446 return 0; 447 } 448 449 static int disable_camera(struct bcm2835_mmal_dev *dev) 450 { 451 int ret; 452 453 if (!dev->camera_use_count) { 454 v4l2_err(&dev->v4l2_dev, 455 "Disabled the camera when already disabled\n"); 456 return -EINVAL; 457 } 458 dev->camera_use_count--; 459 if (!dev->camera_use_count) { 460 unsigned int i = 0xFFFFFFFF; 461 462 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 463 "Disabling camera\n"); 464 ret = vchiq_mmal_component_disable(dev->instance, 465 dev->component[COMP_CAMERA]); 466 if (ret < 0) { 467 v4l2_err(&dev->v4l2_dev, 468 "Failed disabling camera, ret %d\n", ret); 469 return -EINVAL; 470 } 471 vchiq_mmal_port_parameter_set( 472 dev->instance, 473 &dev->component[COMP_CAMERA]->control, 474 MMAL_PARAMETER_CAMERA_NUM, &i, 475 sizeof(i)); 476 } 477 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 478 "Camera refcount now %d\n", dev->camera_use_count); 479 return 0; 480 } 481 482 static void buffer_queue(struct vb2_buffer *vb) 483 { 484 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue); 485 struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb); 486 struct vb2_mmal_buffer *buf = 487 container_of(vb2, struct vb2_mmal_buffer, vb); 488 int ret; 489 490 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 491 "%s: dev:%p buf:%p, idx %u\n", 492 __func__, dev, buf, vb2->vb2_buf.index); 493 494 ret = vchiq_mmal_submit_buffer(dev->instance, dev->capture.port, 495 &buf->mmal); 496 if (ret < 0) 497 v4l2_err(&dev->v4l2_dev, "%s: error submitting buffer\n", 498 __func__); 499 } 500 501 static int start_streaming(struct vb2_queue *vq, unsigned int count) 502 { 503 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq); 504 int ret; 505 u32 parameter_size; 506 507 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n", 508 __func__, dev); 509 510 /* ensure a format has actually been set */ 511 if (!dev->capture.port) 512 return -EINVAL; 513 514 if (enable_camera(dev) < 0) { 515 v4l2_err(&dev->v4l2_dev, "Failed to enable camera\n"); 516 return -EINVAL; 517 } 518 519 /*init_completion(&dev->capture.frame_cmplt); */ 520 521 /* enable frame capture */ 522 dev->capture.frame_count = 1; 523 524 /* reset sequence number */ 525 dev->capture.sequence = 0; 526 527 /* if the preview is not already running, wait for a few frames for AGC 528 * to settle down. 529 */ 530 if (!dev->component[COMP_PREVIEW]->enabled) 531 msleep(300); 532 533 /* enable the connection from camera to encoder (if applicable) */ 534 if (dev->capture.camera_port != dev->capture.port && 535 dev->capture.camera_port) { 536 ret = vchiq_mmal_port_enable(dev->instance, 537 dev->capture.camera_port, NULL); 538 if (ret) { 539 v4l2_err(&dev->v4l2_dev, 540 "Failed to enable encode tunnel - error %d\n", 541 ret); 542 return -1; 543 } 544 } 545 546 /* Get VC timestamp at this point in time */ 547 parameter_size = sizeof(dev->capture.vc_start_timestamp); 548 if (vchiq_mmal_port_parameter_get(dev->instance, 549 dev->capture.camera_port, 550 MMAL_PARAMETER_SYSTEM_TIME, 551 &dev->capture.vc_start_timestamp, 552 ¶meter_size)) { 553 v4l2_err(&dev->v4l2_dev, 554 "Failed to get VC start time - update your VC f/w\n"); 555 556 /* Flag to indicate just to rely on kernel timestamps */ 557 dev->capture.vc_start_timestamp = -1; 558 } else { 559 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 560 "Start time %lld size %d\n", 561 dev->capture.vc_start_timestamp, parameter_size); 562 } 563 564 dev->capture.kernel_start_ts = ktime_get(); 565 566 /* enable the camera port */ 567 dev->capture.port->cb_ctx = dev; 568 ret = vchiq_mmal_port_enable(dev->instance, dev->capture.port, 569 buffer_cb); 570 if (ret) { 571 v4l2_err(&dev->v4l2_dev, 572 "Failed to enable capture port - error %d. Disabling camera port again\n", 573 ret); 574 575 vchiq_mmal_port_disable(dev->instance, 576 dev->capture.camera_port); 577 if (disable_camera(dev) < 0) { 578 v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n"); 579 return -EINVAL; 580 } 581 return -1; 582 } 583 584 /* capture the first frame */ 585 vchiq_mmal_port_parameter_set(dev->instance, 586 dev->capture.camera_port, 587 MMAL_PARAMETER_CAPTURE, 588 &dev->capture.frame_count, 589 sizeof(dev->capture.frame_count)); 590 return 0; 591 } 592 593 /* abort streaming and wait for last buffer */ 594 static void stop_streaming(struct vb2_queue *vq) 595 { 596 int ret; 597 unsigned long timeout; 598 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq); 599 struct vchiq_mmal_port *port = dev->capture.port; 600 601 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n", 602 __func__, dev); 603 604 init_completion(&dev->capture.frame_cmplt); 605 dev->capture.frame_count = 0; 606 607 /* ensure a format has actually been set */ 608 if (!port) { 609 v4l2_err(&dev->v4l2_dev, 610 "no capture port - stream not started?\n"); 611 return; 612 } 613 614 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "stopping capturing\n"); 615 616 /* stop capturing frames */ 617 vchiq_mmal_port_parameter_set(dev->instance, 618 dev->capture.camera_port, 619 MMAL_PARAMETER_CAPTURE, 620 &dev->capture.frame_count, 621 sizeof(dev->capture.frame_count)); 622 623 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 624 "disabling connection\n"); 625 626 /* disable the connection from camera to encoder */ 627 ret = vchiq_mmal_port_disable(dev->instance, dev->capture.camera_port); 628 if (!ret && dev->capture.camera_port != port) { 629 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 630 "disabling port\n"); 631 ret = vchiq_mmal_port_disable(dev->instance, port); 632 } else if (dev->capture.camera_port != port) { 633 v4l2_err(&dev->v4l2_dev, "port_disable failed, error %d\n", 634 ret); 635 } 636 637 /* wait for all buffers to be returned */ 638 while (atomic_read(&port->buffers_with_vpu)) { 639 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 640 "%s: Waiting for buffers to be returned - %d outstanding\n", 641 __func__, atomic_read(&port->buffers_with_vpu)); 642 timeout = wait_for_completion_timeout(&dev->capture.frame_cmplt, 643 HZ); 644 if (timeout == 0) { 645 v4l2_err(&dev->v4l2_dev, "%s: Timeout waiting for buffers to be returned - %d outstanding\n", 646 __func__, 647 atomic_read(&port->buffers_with_vpu)); 648 break; 649 } 650 } 651 652 if (disable_camera(dev) < 0) 653 v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n"); 654 } 655 656 static const struct vb2_ops bcm2835_mmal_video_qops = { 657 .queue_setup = queue_setup, 658 .buf_init = buffer_init, 659 .buf_prepare = buffer_prepare, 660 .buf_cleanup = buffer_cleanup, 661 .buf_queue = buffer_queue, 662 .start_streaming = start_streaming, 663 .stop_streaming = stop_streaming, 664 .wait_prepare = vb2_ops_wait_prepare, 665 .wait_finish = vb2_ops_wait_finish, 666 }; 667 668 /* ------------------------------------------------------------------ 669 * IOCTL operations 670 * ------------------------------------------------------------------ 671 */ 672 673 static int set_overlay_params(struct bcm2835_mmal_dev *dev, 674 struct vchiq_mmal_port *port) 675 { 676 struct mmal_parameter_displayregion prev_config = { 677 .set = MMAL_DISPLAY_SET_LAYER | 678 MMAL_DISPLAY_SET_ALPHA | 679 MMAL_DISPLAY_SET_DEST_RECT | 680 MMAL_DISPLAY_SET_FULLSCREEN, 681 .layer = 2, 682 .alpha = dev->overlay.global_alpha, 683 .fullscreen = 0, 684 .dest_rect = { 685 .x = dev->overlay.w.left, 686 .y = dev->overlay.w.top, 687 .width = dev->overlay.w.width, 688 .height = dev->overlay.w.height, 689 }, 690 }; 691 return vchiq_mmal_port_parameter_set(dev->instance, port, 692 MMAL_PARAMETER_DISPLAYREGION, 693 &prev_config, sizeof(prev_config)); 694 } 695 696 /* overlay ioctl */ 697 static int vidioc_enum_fmt_vid_overlay(struct file *file, void *priv, 698 struct v4l2_fmtdesc *f) 699 { 700 struct mmal_fmt *fmt; 701 702 if (f->index >= ARRAY_SIZE(formats)) 703 return -EINVAL; 704 705 fmt = &formats[f->index]; 706 707 f->pixelformat = fmt->fourcc; 708 709 return 0; 710 } 711 712 static int vidioc_g_fmt_vid_overlay(struct file *file, void *priv, 713 struct v4l2_format *f) 714 { 715 struct bcm2835_mmal_dev *dev = video_drvdata(file); 716 717 f->fmt.win = dev->overlay; 718 719 return 0; 720 } 721 722 static int vidioc_try_fmt_vid_overlay(struct file *file, void *priv, 723 struct v4l2_format *f) 724 { 725 struct bcm2835_mmal_dev *dev = video_drvdata(file); 726 727 f->fmt.win.field = V4L2_FIELD_NONE; 728 f->fmt.win.chromakey = 0; 729 f->fmt.win.clips = NULL; 730 f->fmt.win.clipcount = 0; 731 f->fmt.win.bitmap = NULL; 732 733 v4l_bound_align_image(&f->fmt.win.w.width, MIN_WIDTH, dev->max_width, 1, 734 &f->fmt.win.w.height, MIN_HEIGHT, dev->max_height, 735 1, 0); 736 v4l_bound_align_image(&f->fmt.win.w.left, MIN_WIDTH, dev->max_width, 1, 737 &f->fmt.win.w.top, MIN_HEIGHT, dev->max_height, 738 1, 0); 739 740 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 741 "Overlay: Now w/h %dx%d l/t %dx%d\n", 742 f->fmt.win.w.width, f->fmt.win.w.height, 743 f->fmt.win.w.left, f->fmt.win.w.top); 744 745 v4l2_dump_win_format(1, 746 bcm2835_v4l2_debug, 747 &dev->v4l2_dev, 748 &f->fmt.win, 749 __func__); 750 return 0; 751 } 752 753 static int vidioc_s_fmt_vid_overlay(struct file *file, void *priv, 754 struct v4l2_format *f) 755 { 756 struct bcm2835_mmal_dev *dev = video_drvdata(file); 757 758 vidioc_try_fmt_vid_overlay(file, priv, f); 759 760 dev->overlay = f->fmt.win; 761 if (dev->component[COMP_PREVIEW]->enabled) { 762 set_overlay_params(dev, 763 &dev->component[COMP_PREVIEW]->input[0]); 764 } 765 766 return 0; 767 } 768 769 static int vidioc_overlay(struct file *file, void *f, unsigned int on) 770 { 771 int ret; 772 struct bcm2835_mmal_dev *dev = video_drvdata(file); 773 struct vchiq_mmal_port *src; 774 struct vchiq_mmal_port *dst; 775 776 if ((on && dev->component[COMP_PREVIEW]->enabled) || 777 (!on && !dev->component[COMP_PREVIEW]->enabled)) 778 return 0; /* already in requested state */ 779 780 src = &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW]; 781 782 if (!on) { 783 /* disconnect preview ports and disable component */ 784 ret = vchiq_mmal_port_disable(dev->instance, src); 785 if (!ret) 786 ret = vchiq_mmal_port_connect_tunnel(dev->instance, src, 787 NULL); 788 if (ret >= 0) 789 ret = vchiq_mmal_component_disable( 790 dev->instance, 791 dev->component[COMP_PREVIEW]); 792 793 disable_camera(dev); 794 return ret; 795 } 796 797 /* set preview port format and connect it to output */ 798 dst = &dev->component[COMP_PREVIEW]->input[0]; 799 800 ret = vchiq_mmal_port_set_format(dev->instance, src); 801 if (ret < 0) 802 return ret; 803 804 ret = set_overlay_params(dev, dst); 805 if (ret < 0) 806 return ret; 807 808 if (enable_camera(dev) < 0) 809 return -EINVAL; 810 811 ret = vchiq_mmal_component_enable(dev->instance, 812 dev->component[COMP_PREVIEW]); 813 if (ret < 0) 814 return ret; 815 816 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "connecting %p to %p\n", 817 src, dst); 818 ret = vchiq_mmal_port_connect_tunnel(dev->instance, src, dst); 819 if (ret) 820 return ret; 821 822 return vchiq_mmal_port_enable(dev->instance, src, NULL); 823 } 824 825 static int vidioc_g_fbuf(struct file *file, void *fh, 826 struct v4l2_framebuffer *a) 827 { 828 /* The video overlay must stay within the framebuffer and can't be 829 * positioned independently. 830 */ 831 struct bcm2835_mmal_dev *dev = video_drvdata(file); 832 struct vchiq_mmal_port *preview_port = 833 &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW]; 834 835 a->capability = V4L2_FBUF_CAP_EXTERNOVERLAY | 836 V4L2_FBUF_CAP_GLOBAL_ALPHA; 837 a->flags = V4L2_FBUF_FLAG_OVERLAY; 838 a->fmt.width = preview_port->es.video.width; 839 a->fmt.height = preview_port->es.video.height; 840 a->fmt.pixelformat = V4L2_PIX_FMT_YUV420; 841 a->fmt.bytesperline = preview_port->es.video.width; 842 a->fmt.sizeimage = (preview_port->es.video.width * 843 preview_port->es.video.height * 3) >> 1; 844 a->fmt.colorspace = V4L2_COLORSPACE_SMPTE170M; 845 846 return 0; 847 } 848 849 /* input ioctls */ 850 static int vidioc_enum_input(struct file *file, void *priv, 851 struct v4l2_input *inp) 852 { 853 /* only a single camera input */ 854 if (inp->index) 855 return -EINVAL; 856 857 inp->type = V4L2_INPUT_TYPE_CAMERA; 858 snprintf((char *)inp->name, sizeof(inp->name), "Camera %u", inp->index); 859 return 0; 860 } 861 862 static int vidioc_g_input(struct file *file, void *priv, unsigned int *i) 863 { 864 *i = 0; 865 return 0; 866 } 867 868 static int vidioc_s_input(struct file *file, void *priv, unsigned int i) 869 { 870 if (i) 871 return -EINVAL; 872 873 return 0; 874 } 875 876 /* capture ioctls */ 877 static int vidioc_querycap(struct file *file, void *priv, 878 struct v4l2_capability *cap) 879 { 880 struct bcm2835_mmal_dev *dev = video_drvdata(file); 881 u32 major; 882 u32 minor; 883 884 vchiq_mmal_version(dev->instance, &major, &minor); 885 886 strscpy(cap->driver, "bcm2835 mmal", sizeof(cap->driver)); 887 snprintf((char *)cap->card, sizeof(cap->card), "mmal service %d.%d", major, minor); 888 889 snprintf((char *)cap->bus_info, sizeof(cap->bus_info), "platform:%s", dev->v4l2_dev.name); 890 return 0; 891 } 892 893 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv, 894 struct v4l2_fmtdesc *f) 895 { 896 struct mmal_fmt *fmt; 897 898 if (f->index >= ARRAY_SIZE(formats)) 899 return -EINVAL; 900 901 fmt = &formats[f->index]; 902 903 f->pixelformat = fmt->fourcc; 904 905 return 0; 906 } 907 908 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv, 909 struct v4l2_format *f) 910 { 911 struct bcm2835_mmal_dev *dev = video_drvdata(file); 912 913 f->fmt.pix.width = dev->capture.width; 914 f->fmt.pix.height = dev->capture.height; 915 f->fmt.pix.field = V4L2_FIELD_NONE; 916 f->fmt.pix.pixelformat = dev->capture.fmt->fourcc; 917 f->fmt.pix.bytesperline = dev->capture.stride; 918 f->fmt.pix.sizeimage = dev->capture.buffersize; 919 920 if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_RGB24) 921 f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB; 922 else if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_JPEG) 923 f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG; 924 else 925 f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; 926 f->fmt.pix.priv = 0; 927 928 v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix, 929 __func__); 930 return 0; 931 } 932 933 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv, 934 struct v4l2_format *f) 935 { 936 struct bcm2835_mmal_dev *dev = video_drvdata(file); 937 struct mmal_fmt *mfmt; 938 939 mfmt = get_format(f); 940 if (!mfmt) { 941 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 942 "Fourcc format (0x%08x) unknown.\n", 943 f->fmt.pix.pixelformat); 944 f->fmt.pix.pixelformat = formats[0].fourcc; 945 mfmt = get_format(f); 946 } 947 948 f->fmt.pix.field = V4L2_FIELD_NONE; 949 950 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 951 "Clipping/aligning %dx%d format %08X\n", 952 f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat); 953 954 v4l_bound_align_image(&f->fmt.pix.width, MIN_WIDTH, dev->max_width, 1, 955 &f->fmt.pix.height, MIN_HEIGHT, dev->max_height, 956 1, 0); 957 f->fmt.pix.bytesperline = f->fmt.pix.width * mfmt->ybbp; 958 if (!mfmt->remove_padding) { 959 if (mfmt->depth == 24) { 960 /* 961 * 24bpp is a pain as we can't use simple masking. 962 * Min stride is width aligned to 16, times 24bpp. 963 */ 964 f->fmt.pix.bytesperline = 965 ((f->fmt.pix.width + 15) & ~15) * 3; 966 } else { 967 /* 968 * GPU isn't removing padding, so stride is aligned to 969 * 32 970 */ 971 int align_mask = ((32 * mfmt->depth) >> 3) - 1; 972 973 f->fmt.pix.bytesperline = 974 (f->fmt.pix.bytesperline + align_mask) & 975 ~align_mask; 976 } 977 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 978 "Not removing padding, so bytes/line = %d\n", 979 f->fmt.pix.bytesperline); 980 } 981 982 /* Image buffer has to be padded to allow for alignment, even though 983 * we sometimes then remove that padding before delivering the buffer. 984 */ 985 f->fmt.pix.sizeimage = ((f->fmt.pix.height + 15) & ~15) * 986 (((f->fmt.pix.width + 31) & ~31) * mfmt->depth) >> 3; 987 988 if ((mfmt->flags & V4L2_FMT_FLAG_COMPRESSED) && 989 f->fmt.pix.sizeimage < MIN_BUFFER_SIZE) 990 f->fmt.pix.sizeimage = MIN_BUFFER_SIZE; 991 992 if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24) 993 f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB; 994 else if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG) 995 f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG; 996 else 997 f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; 998 f->fmt.pix.priv = 0; 999 1000 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 1001 "Now %dx%d format %08X\n", 1002 f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat); 1003 1004 v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix, 1005 __func__); 1006 return 0; 1007 } 1008 1009 1010 static int mmal_setup_video_component(struct bcm2835_mmal_dev *dev, 1011 struct v4l2_format *f) 1012 { 1013 bool overlay_enabled = !!dev->component[COMP_PREVIEW]->enabled; 1014 struct vchiq_mmal_port *preview_port; 1015 int ret; 1016 1017 preview_port = &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW]; 1018 1019 /* Preview and encode ports need to match on resolution */ 1020 if (overlay_enabled) { 1021 /* Need to disable the overlay before we can update 1022 * the resolution 1023 */ 1024 ret = vchiq_mmal_port_disable(dev->instance, preview_port); 1025 if (!ret) { 1026 ret = vchiq_mmal_port_connect_tunnel(dev->instance, 1027 preview_port, 1028 NULL); 1029 } 1030 } 1031 preview_port->es.video.width = f->fmt.pix.width; 1032 preview_port->es.video.height = f->fmt.pix.height; 1033 preview_port->es.video.crop.x = 0; 1034 preview_port->es.video.crop.y = 0; 1035 preview_port->es.video.crop.width = f->fmt.pix.width; 1036 preview_port->es.video.crop.height = f->fmt.pix.height; 1037 preview_port->es.video.frame_rate.numerator = 1038 dev->capture.timeperframe.denominator; 1039 preview_port->es.video.frame_rate.denominator = 1040 dev->capture.timeperframe.numerator; 1041 ret = vchiq_mmal_port_set_format(dev->instance, preview_port); 1042 1043 if (overlay_enabled) { 1044 ret = vchiq_mmal_port_connect_tunnel(dev->instance, 1045 preview_port, 1046 &dev->component[COMP_PREVIEW]->input[0]); 1047 if (ret) 1048 return ret; 1049 1050 ret = vchiq_mmal_port_enable(dev->instance, preview_port, NULL); 1051 } 1052 1053 return ret; 1054 } 1055 1056 static int mmal_setup_encode_component(struct bcm2835_mmal_dev *dev, 1057 struct v4l2_format *f, 1058 struct vchiq_mmal_port *port, 1059 struct vchiq_mmal_port *camera_port, 1060 struct vchiq_mmal_component *component) 1061 { 1062 struct mmal_fmt *mfmt = get_format(f); 1063 int ret; 1064 1065 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 1066 "vid_cap - set up encode comp\n"); 1067 1068 /* configure buffering */ 1069 camera_port->current_buffer.size = camera_port->recommended_buffer.size; 1070 camera_port->current_buffer.num = camera_port->recommended_buffer.num; 1071 1072 ret = vchiq_mmal_port_connect_tunnel(dev->instance, camera_port, 1073 &component->input[0]); 1074 if (ret) { 1075 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 1076 "%s failed to create connection\n", __func__); 1077 /* ensure capture is not going to be tried */ 1078 dev->capture.port = NULL; 1079 return ret; 1080 } 1081 1082 port->es.video.width = f->fmt.pix.width; 1083 port->es.video.height = f->fmt.pix.height; 1084 port->es.video.crop.x = 0; 1085 port->es.video.crop.y = 0; 1086 port->es.video.crop.width = f->fmt.pix.width; 1087 port->es.video.crop.height = f->fmt.pix.height; 1088 port->es.video.frame_rate.numerator = 1089 dev->capture.timeperframe.denominator; 1090 port->es.video.frame_rate.denominator = 1091 dev->capture.timeperframe.numerator; 1092 1093 port->format.encoding = mfmt->mmal; 1094 port->format.encoding_variant = 0; 1095 /* Set any encoding specific parameters */ 1096 switch (mfmt->mmal_component) { 1097 case COMP_VIDEO_ENCODE: 1098 port->format.bitrate = dev->capture.encode_bitrate; 1099 break; 1100 case COMP_IMAGE_ENCODE: 1101 /* Could set EXIF parameters here */ 1102 break; 1103 default: 1104 break; 1105 } 1106 1107 ret = vchiq_mmal_port_set_format(dev->instance, port); 1108 if (ret) { 1109 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 1110 "%s failed to set format %dx%d fmt %08X\n", 1111 __func__, 1112 f->fmt.pix.width, 1113 f->fmt.pix.height, 1114 f->fmt.pix.pixelformat); 1115 return ret; 1116 } 1117 1118 ret = vchiq_mmal_component_enable(dev->instance, component); 1119 if (ret) { 1120 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 1121 "%s Failed to enable encode components\n", __func__); 1122 return ret; 1123 } 1124 1125 /* configure buffering */ 1126 port->current_buffer.num = 1; 1127 port->current_buffer.size = f->fmt.pix.sizeimage; 1128 if (port->format.encoding == MMAL_ENCODING_JPEG) { 1129 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 1130 "JPG - buf size now %d was %d\n", 1131 f->fmt.pix.sizeimage, 1132 port->current_buffer.size); 1133 port->current_buffer.size = 1134 (f->fmt.pix.sizeimage < (100 << 10)) ? 1135 (100 << 10) : f->fmt.pix.sizeimage; 1136 } 1137 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 1138 "vid_cap - cur_buf.size set to %d\n", f->fmt.pix.sizeimage); 1139 port->current_buffer.alignment = 0; 1140 1141 return 0; 1142 } 1143 1144 static int mmal_setup_components(struct bcm2835_mmal_dev *dev, 1145 struct v4l2_format *f) 1146 { 1147 int ret; 1148 struct vchiq_mmal_port *port = NULL, *camera_port = NULL; 1149 struct vchiq_mmal_component *encode_component = NULL; 1150 struct mmal_fmt *mfmt = get_format(f); 1151 bool remove_padding; 1152 1153 if (!mfmt) 1154 return -EINVAL; 1155 1156 if (dev->capture.encode_component) { 1157 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 1158 "vid_cap - disconnect previous tunnel\n"); 1159 1160 /* Disconnect any previous connection */ 1161 vchiq_mmal_port_connect_tunnel(dev->instance, 1162 dev->capture.camera_port, NULL); 1163 dev->capture.camera_port = NULL; 1164 ret = vchiq_mmal_component_disable(dev->instance, 1165 dev->capture.encode_component); 1166 if (ret) 1167 v4l2_err(&dev->v4l2_dev, 1168 "Failed to disable encode component %d\n", 1169 ret); 1170 1171 dev->capture.encode_component = NULL; 1172 } 1173 /* format dependent port setup */ 1174 switch (mfmt->mmal_component) { 1175 case COMP_CAMERA: 1176 /* Make a further decision on port based on resolution */ 1177 if (f->fmt.pix.width <= max_video_width && 1178 f->fmt.pix.height <= max_video_height) 1179 camera_port = 1180 &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO]; 1181 else 1182 camera_port = 1183 &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE]; 1184 port = camera_port; 1185 break; 1186 case COMP_IMAGE_ENCODE: 1187 encode_component = dev->component[COMP_IMAGE_ENCODE]; 1188 port = &dev->component[COMP_IMAGE_ENCODE]->output[0]; 1189 camera_port = 1190 &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE]; 1191 break; 1192 case COMP_VIDEO_ENCODE: 1193 encode_component = dev->component[COMP_VIDEO_ENCODE]; 1194 port = &dev->component[COMP_VIDEO_ENCODE]->output[0]; 1195 camera_port = 1196 &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO]; 1197 break; 1198 default: 1199 break; 1200 } 1201 1202 if (!port) 1203 return -EINVAL; 1204 1205 if (encode_component) 1206 camera_port->format.encoding = MMAL_ENCODING_OPAQUE; 1207 else 1208 camera_port->format.encoding = mfmt->mmal; 1209 1210 if (dev->rgb_bgr_swapped) { 1211 if (camera_port->format.encoding == MMAL_ENCODING_RGB24) 1212 camera_port->format.encoding = MMAL_ENCODING_BGR24; 1213 else if (camera_port->format.encoding == MMAL_ENCODING_BGR24) 1214 camera_port->format.encoding = MMAL_ENCODING_RGB24; 1215 } 1216 1217 remove_padding = mfmt->remove_padding; 1218 vchiq_mmal_port_parameter_set(dev->instance, camera_port, 1219 MMAL_PARAMETER_NO_IMAGE_PADDING, 1220 &remove_padding, sizeof(remove_padding)); 1221 1222 camera_port->format.encoding_variant = 0; 1223 camera_port->es.video.width = f->fmt.pix.width; 1224 camera_port->es.video.height = f->fmt.pix.height; 1225 camera_port->es.video.crop.x = 0; 1226 camera_port->es.video.crop.y = 0; 1227 camera_port->es.video.crop.width = f->fmt.pix.width; 1228 camera_port->es.video.crop.height = f->fmt.pix.height; 1229 camera_port->es.video.frame_rate.numerator = 0; 1230 camera_port->es.video.frame_rate.denominator = 1; 1231 camera_port->es.video.color_space = MMAL_COLOR_SPACE_JPEG_JFIF; 1232 1233 ret = vchiq_mmal_port_set_format(dev->instance, camera_port); 1234 1235 if (!ret && 1236 camera_port == 1237 &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO]) { 1238 ret = mmal_setup_video_component(dev, f); 1239 } 1240 1241 if (ret) { 1242 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 1243 "%s failed to set format %dx%d %08X\n", __func__, 1244 f->fmt.pix.width, f->fmt.pix.height, 1245 f->fmt.pix.pixelformat); 1246 /* ensure capture is not going to be tried */ 1247 dev->capture.port = NULL; 1248 return ret; 1249 } 1250 1251 if (encode_component) { 1252 ret = mmal_setup_encode_component(dev, f, port, 1253 camera_port, 1254 encode_component); 1255 1256 if (ret) 1257 return ret; 1258 } else { 1259 /* configure buffering */ 1260 camera_port->current_buffer.num = 1; 1261 camera_port->current_buffer.size = f->fmt.pix.sizeimage; 1262 camera_port->current_buffer.alignment = 0; 1263 } 1264 1265 dev->capture.fmt = mfmt; 1266 dev->capture.stride = f->fmt.pix.bytesperline; 1267 dev->capture.width = camera_port->es.video.crop.width; 1268 dev->capture.height = camera_port->es.video.crop.height; 1269 dev->capture.buffersize = port->current_buffer.size; 1270 1271 /* select port for capture */ 1272 dev->capture.port = port; 1273 dev->capture.camera_port = camera_port; 1274 dev->capture.encode_component = encode_component; 1275 v4l2_dbg(1, bcm2835_v4l2_debug, 1276 &dev->v4l2_dev, 1277 "Set dev->capture.fmt %08X, %dx%d, stride %d, size %d", 1278 port->format.encoding, 1279 dev->capture.width, dev->capture.height, 1280 dev->capture.stride, dev->capture.buffersize); 1281 1282 /* todo: Need to convert the vchiq/mmal error into a v4l2 error. */ 1283 return ret; 1284 } 1285 1286 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv, 1287 struct v4l2_format *f) 1288 { 1289 int ret; 1290 struct bcm2835_mmal_dev *dev = video_drvdata(file); 1291 struct mmal_fmt *mfmt; 1292 1293 /* try the format to set valid parameters */ 1294 ret = vidioc_try_fmt_vid_cap(file, priv, f); 1295 if (ret) { 1296 v4l2_err(&dev->v4l2_dev, 1297 "vid_cap - vidioc_try_fmt_vid_cap failed\n"); 1298 return ret; 1299 } 1300 1301 /* if a capture is running refuse to set format */ 1302 if (vb2_is_busy(&dev->capture.vb_vidq)) { 1303 v4l2_info(&dev->v4l2_dev, "%s device busy\n", __func__); 1304 return -EBUSY; 1305 } 1306 1307 /* If the format is unsupported v4l2 says we should switch to 1308 * a supported one and not return an error. 1309 */ 1310 mfmt = get_format(f); 1311 if (!mfmt) { 1312 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 1313 "Fourcc format (0x%08x) unknown.\n", 1314 f->fmt.pix.pixelformat); 1315 f->fmt.pix.pixelformat = formats[0].fourcc; 1316 mfmt = get_format(f); 1317 } 1318 1319 ret = mmal_setup_components(dev, f); 1320 if (ret) { 1321 v4l2_err(&dev->v4l2_dev, 1322 "%s: failed to setup mmal components: %d\n", 1323 __func__, ret); 1324 ret = -EINVAL; 1325 } 1326 1327 return ret; 1328 } 1329 1330 static int vidioc_enum_framesizes(struct file *file, void *fh, 1331 struct v4l2_frmsizeenum *fsize) 1332 { 1333 struct bcm2835_mmal_dev *dev = video_drvdata(file); 1334 static const struct v4l2_frmsize_stepwise sizes = { 1335 MIN_WIDTH, 0, 2, 1336 MIN_HEIGHT, 0, 2 1337 }; 1338 int i; 1339 1340 if (fsize->index) 1341 return -EINVAL; 1342 for (i = 0; i < ARRAY_SIZE(formats); i++) 1343 if (formats[i].fourcc == fsize->pixel_format) 1344 break; 1345 if (i == ARRAY_SIZE(formats)) 1346 return -EINVAL; 1347 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE; 1348 fsize->stepwise = sizes; 1349 fsize->stepwise.max_width = dev->max_width; 1350 fsize->stepwise.max_height = dev->max_height; 1351 return 0; 1352 } 1353 1354 /* timeperframe is arbitrary and continuous */ 1355 static int vidioc_enum_frameintervals(struct file *file, void *priv, 1356 struct v4l2_frmivalenum *fival) 1357 { 1358 struct bcm2835_mmal_dev *dev = video_drvdata(file); 1359 int i; 1360 1361 if (fival->index) 1362 return -EINVAL; 1363 1364 for (i = 0; i < ARRAY_SIZE(formats); i++) 1365 if (formats[i].fourcc == fival->pixel_format) 1366 break; 1367 if (i == ARRAY_SIZE(formats)) 1368 return -EINVAL; 1369 1370 /* regarding width & height - we support any within range */ 1371 if (fival->width < MIN_WIDTH || fival->width > dev->max_width || 1372 fival->height < MIN_HEIGHT || fival->height > dev->max_height) 1373 return -EINVAL; 1374 1375 fival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS; 1376 1377 /* fill in stepwise (step=1.0 is required by V4L2 spec) */ 1378 fival->stepwise.min = tpf_min; 1379 fival->stepwise.max = tpf_max; 1380 fival->stepwise.step = (struct v4l2_fract) {1, 1}; 1381 1382 return 0; 1383 } 1384 1385 static int vidioc_g_parm(struct file *file, void *priv, 1386 struct v4l2_streamparm *parm) 1387 { 1388 struct bcm2835_mmal_dev *dev = video_drvdata(file); 1389 1390 if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) 1391 return -EINVAL; 1392 1393 parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME; 1394 parm->parm.capture.timeperframe = dev->capture.timeperframe; 1395 parm->parm.capture.readbuffers = 1; 1396 return 0; 1397 } 1398 1399 static int vidioc_s_parm(struct file *file, void *priv, 1400 struct v4l2_streamparm *parm) 1401 { 1402 struct bcm2835_mmal_dev *dev = video_drvdata(file); 1403 struct v4l2_fract tpf; 1404 1405 if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) 1406 return -EINVAL; 1407 1408 tpf = parm->parm.capture.timeperframe; 1409 1410 /* tpf: {*, 0} resets timing; clip to [min, max]*/ 1411 tpf = tpf.denominator ? tpf : tpf_default; 1412 tpf = V4L2_FRACT_COMPARE(tpf, <, tpf_min) ? tpf_min : tpf; 1413 tpf = V4L2_FRACT_COMPARE(tpf, >, tpf_max) ? tpf_max : tpf; 1414 1415 dev->capture.timeperframe = tpf; 1416 parm->parm.capture.timeperframe = tpf; 1417 parm->parm.capture.readbuffers = 1; 1418 parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME; 1419 1420 set_framerate_params(dev); 1421 1422 return 0; 1423 } 1424 1425 static const struct v4l2_ioctl_ops camera0_ioctl_ops = { 1426 /* overlay */ 1427 .vidioc_enum_fmt_vid_overlay = vidioc_enum_fmt_vid_overlay, 1428 .vidioc_g_fmt_vid_overlay = vidioc_g_fmt_vid_overlay, 1429 .vidioc_try_fmt_vid_overlay = vidioc_try_fmt_vid_overlay, 1430 .vidioc_s_fmt_vid_overlay = vidioc_s_fmt_vid_overlay, 1431 .vidioc_overlay = vidioc_overlay, 1432 .vidioc_g_fbuf = vidioc_g_fbuf, 1433 1434 /* inputs */ 1435 .vidioc_enum_input = vidioc_enum_input, 1436 .vidioc_g_input = vidioc_g_input, 1437 .vidioc_s_input = vidioc_s_input, 1438 1439 /* capture */ 1440 .vidioc_querycap = vidioc_querycap, 1441 .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap, 1442 .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap, 1443 .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap, 1444 .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap, 1445 1446 /* buffer management */ 1447 .vidioc_reqbufs = vb2_ioctl_reqbufs, 1448 .vidioc_create_bufs = vb2_ioctl_create_bufs, 1449 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 1450 .vidioc_querybuf = vb2_ioctl_querybuf, 1451 .vidioc_qbuf = vb2_ioctl_qbuf, 1452 .vidioc_dqbuf = vb2_ioctl_dqbuf, 1453 .vidioc_enum_framesizes = vidioc_enum_framesizes, 1454 .vidioc_enum_frameintervals = vidioc_enum_frameintervals, 1455 .vidioc_g_parm = vidioc_g_parm, 1456 .vidioc_s_parm = vidioc_s_parm, 1457 .vidioc_streamon = vb2_ioctl_streamon, 1458 .vidioc_streamoff = vb2_ioctl_streamoff, 1459 1460 .vidioc_log_status = v4l2_ctrl_log_status, 1461 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event, 1462 .vidioc_unsubscribe_event = v4l2_event_unsubscribe, 1463 }; 1464 1465 /* ------------------------------------------------------------------ 1466 * Driver init/finalise 1467 * ------------------------------------------------------------------ 1468 */ 1469 1470 static const struct v4l2_file_operations camera0_fops = { 1471 .owner = THIS_MODULE, 1472 .open = v4l2_fh_open, 1473 .release = vb2_fop_release, 1474 .read = vb2_fop_read, 1475 .poll = vb2_fop_poll, 1476 .unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */ 1477 .mmap = vb2_fop_mmap, 1478 }; 1479 1480 static const struct video_device vdev_template = { 1481 .name = "camera0", 1482 .fops = &camera0_fops, 1483 .ioctl_ops = &camera0_ioctl_ops, 1484 .release = video_device_release_empty, 1485 .device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY | 1486 V4L2_CAP_STREAMING | V4L2_CAP_READWRITE, 1487 }; 1488 1489 /* Returns the number of cameras, and also the max resolution supported 1490 * by those cameras. 1491 */ 1492 static int get_num_cameras(struct vchiq_mmal_instance *instance, 1493 unsigned int resolutions[][2], int num_resolutions) 1494 { 1495 int ret; 1496 struct vchiq_mmal_component *cam_info_component; 1497 struct mmal_parameter_camera_info cam_info = {0}; 1498 u32 param_size = sizeof(cam_info); 1499 int i; 1500 1501 /* create a camera_info component */ 1502 ret = vchiq_mmal_component_init(instance, "camera_info", 1503 &cam_info_component); 1504 if (ret < 0) 1505 /* Unusual failure - let's guess one camera. */ 1506 return 1; 1507 1508 if (vchiq_mmal_port_parameter_get(instance, 1509 &cam_info_component->control, 1510 MMAL_PARAMETER_CAMERA_INFO, 1511 &cam_info, 1512 ¶m_size)) { 1513 pr_info("Failed to get camera info\n"); 1514 } 1515 for (i = 0; 1516 i < min_t(unsigned int, cam_info.num_cameras, num_resolutions); 1517 i++) { 1518 resolutions[i][0] = cam_info.cameras[i].max_width; 1519 resolutions[i][1] = cam_info.cameras[i].max_height; 1520 } 1521 1522 vchiq_mmal_component_finalise(instance, 1523 cam_info_component); 1524 1525 return cam_info.num_cameras; 1526 } 1527 1528 static int set_camera_parameters(struct vchiq_mmal_instance *instance, 1529 struct vchiq_mmal_component *camera, 1530 struct bcm2835_mmal_dev *dev) 1531 { 1532 struct mmal_parameter_camera_config cam_config = { 1533 .max_stills_w = dev->max_width, 1534 .max_stills_h = dev->max_height, 1535 .stills_yuv422 = 1, 1536 .one_shot_stills = 1, 1537 .max_preview_video_w = (max_video_width > 1920) ? 1538 max_video_width : 1920, 1539 .max_preview_video_h = (max_video_height > 1088) ? 1540 max_video_height : 1088, 1541 .num_preview_video_frames = 3, 1542 .stills_capture_circular_buffer_height = 0, 1543 .fast_preview_resume = 0, 1544 .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC 1545 }; 1546 1547 return vchiq_mmal_port_parameter_set(instance, &camera->control, 1548 MMAL_PARAMETER_CAMERA_CONFIG, 1549 &cam_config, sizeof(cam_config)); 1550 } 1551 1552 #define MAX_SUPPORTED_ENCODINGS 20 1553 1554 /* MMAL instance and component init */ 1555 static int mmal_init(struct bcm2835_mmal_dev *dev) 1556 { 1557 int ret; 1558 struct mmal_es_format_local *format; 1559 u32 supported_encodings[MAX_SUPPORTED_ENCODINGS]; 1560 u32 param_size; 1561 struct vchiq_mmal_component *camera; 1562 1563 ret = vchiq_mmal_init(&dev->instance); 1564 if (ret < 0) { 1565 v4l2_err(&dev->v4l2_dev, "%s: vchiq mmal init failed %d\n", 1566 __func__, ret); 1567 return ret; 1568 } 1569 1570 /* get the camera component ready */ 1571 ret = vchiq_mmal_component_init(dev->instance, "ril.camera", 1572 &dev->component[COMP_CAMERA]); 1573 if (ret < 0) 1574 goto unreg_mmal; 1575 1576 camera = dev->component[COMP_CAMERA]; 1577 if (camera->outputs < CAM_PORT_COUNT) { 1578 v4l2_err(&dev->v4l2_dev, "%s: too few camera outputs %d needed %d\n", 1579 __func__, camera->outputs, CAM_PORT_COUNT); 1580 ret = -EINVAL; 1581 goto unreg_camera; 1582 } 1583 1584 ret = set_camera_parameters(dev->instance, 1585 camera, 1586 dev); 1587 if (ret < 0) { 1588 v4l2_err(&dev->v4l2_dev, "%s: unable to set camera parameters: %d\n", 1589 __func__, ret); 1590 goto unreg_camera; 1591 } 1592 1593 /* There was an error in the firmware that meant the camera component 1594 * produced BGR instead of RGB. 1595 * This is now fixed, but in order to support the old firmwares, we 1596 * have to check. 1597 */ 1598 dev->rgb_bgr_swapped = true; 1599 param_size = sizeof(supported_encodings); 1600 ret = vchiq_mmal_port_parameter_get(dev->instance, 1601 &camera->output[CAM_PORT_CAPTURE], 1602 MMAL_PARAMETER_SUPPORTED_ENCODINGS, 1603 &supported_encodings, 1604 ¶m_size); 1605 if (ret == 0) { 1606 int i; 1607 1608 for (i = 0; i < param_size / sizeof(u32); i++) { 1609 if (supported_encodings[i] == MMAL_ENCODING_BGR24) { 1610 /* Found BGR24 first - old firmware. */ 1611 break; 1612 } 1613 if (supported_encodings[i] == MMAL_ENCODING_RGB24) { 1614 /* Found RGB24 first 1615 * new firmware, so use RGB24. 1616 */ 1617 dev->rgb_bgr_swapped = false; 1618 break; 1619 } 1620 } 1621 } 1622 format = &camera->output[CAM_PORT_PREVIEW].format; 1623 1624 format->encoding = MMAL_ENCODING_OPAQUE; 1625 format->encoding_variant = MMAL_ENCODING_I420; 1626 1627 format->es->video.width = 1024; 1628 format->es->video.height = 768; 1629 format->es->video.crop.x = 0; 1630 format->es->video.crop.y = 0; 1631 format->es->video.crop.width = 1024; 1632 format->es->video.crop.height = 768; 1633 format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */ 1634 format->es->video.frame_rate.denominator = 1; 1635 1636 format = &camera->output[CAM_PORT_VIDEO].format; 1637 1638 format->encoding = MMAL_ENCODING_OPAQUE; 1639 format->encoding_variant = MMAL_ENCODING_I420; 1640 1641 format->es->video.width = 1024; 1642 format->es->video.height = 768; 1643 format->es->video.crop.x = 0; 1644 format->es->video.crop.y = 0; 1645 format->es->video.crop.width = 1024; 1646 format->es->video.crop.height = 768; 1647 format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */ 1648 format->es->video.frame_rate.denominator = 1; 1649 1650 format = &camera->output[CAM_PORT_CAPTURE].format; 1651 1652 format->encoding = MMAL_ENCODING_OPAQUE; 1653 1654 format->es->video.width = 2592; 1655 format->es->video.height = 1944; 1656 format->es->video.crop.x = 0; 1657 format->es->video.crop.y = 0; 1658 format->es->video.crop.width = 2592; 1659 format->es->video.crop.height = 1944; 1660 format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */ 1661 format->es->video.frame_rate.denominator = 1; 1662 1663 dev->capture.width = format->es->video.width; 1664 dev->capture.height = format->es->video.height; 1665 dev->capture.fmt = &formats[0]; 1666 dev->capture.encode_component = NULL; 1667 dev->capture.timeperframe = tpf_default; 1668 dev->capture.enc_profile = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH; 1669 dev->capture.enc_level = V4L2_MPEG_VIDEO_H264_LEVEL_4_0; 1670 1671 /* get the preview component ready */ 1672 ret = vchiq_mmal_component_init(dev->instance, "ril.video_render", 1673 &dev->component[COMP_PREVIEW]); 1674 if (ret < 0) 1675 goto unreg_camera; 1676 1677 if (dev->component[COMP_PREVIEW]->inputs < 1) { 1678 ret = -EINVAL; 1679 v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n", 1680 __func__, dev->component[COMP_PREVIEW]->inputs, 1); 1681 goto unreg_preview; 1682 } 1683 1684 /* get the image encoder component ready */ 1685 ret = vchiq_mmal_component_init(dev->instance, "ril.image_encode", 1686 &dev->component[COMP_IMAGE_ENCODE]); 1687 if (ret < 0) 1688 goto unreg_preview; 1689 1690 if (dev->component[COMP_IMAGE_ENCODE]->inputs < 1) { 1691 ret = -EINVAL; 1692 v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n", 1693 __func__, dev->component[COMP_IMAGE_ENCODE]->inputs, 1694 1); 1695 goto unreg_image_encoder; 1696 } 1697 1698 /* get the video encoder component ready */ 1699 ret = vchiq_mmal_component_init(dev->instance, "ril.video_encode", 1700 &dev->component[COMP_VIDEO_ENCODE]); 1701 if (ret < 0) 1702 goto unreg_image_encoder; 1703 1704 if (dev->component[COMP_VIDEO_ENCODE]->inputs < 1) { 1705 ret = -EINVAL; 1706 v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n", 1707 __func__, dev->component[COMP_VIDEO_ENCODE]->inputs, 1708 1); 1709 goto unreg_vid_encoder; 1710 } 1711 1712 { 1713 struct vchiq_mmal_port *encoder_port = 1714 &dev->component[COMP_VIDEO_ENCODE]->output[0]; 1715 encoder_port->format.encoding = MMAL_ENCODING_H264; 1716 ret = vchiq_mmal_port_set_format(dev->instance, 1717 encoder_port); 1718 } 1719 1720 { 1721 unsigned int enable = 1; 1722 1723 vchiq_mmal_port_parameter_set( 1724 dev->instance, 1725 &dev->component[COMP_VIDEO_ENCODE]->control, 1726 MMAL_PARAMETER_VIDEO_IMMUTABLE_INPUT, 1727 &enable, sizeof(enable)); 1728 1729 vchiq_mmal_port_parameter_set(dev->instance, 1730 &dev->component[COMP_VIDEO_ENCODE]->control, 1731 MMAL_PARAMETER_MINIMISE_FRAGMENTATION, 1732 &enable, 1733 sizeof(enable)); 1734 } 1735 ret = bcm2835_mmal_set_all_camera_controls(dev); 1736 if (ret < 0) { 1737 v4l2_err(&dev->v4l2_dev, "%s: failed to set all camera controls: %d\n", 1738 __func__, ret); 1739 goto unreg_vid_encoder; 1740 } 1741 1742 return 0; 1743 1744 unreg_vid_encoder: 1745 pr_err("Cleanup: Destroy video encoder\n"); 1746 vchiq_mmal_component_finalise(dev->instance, 1747 dev->component[COMP_VIDEO_ENCODE]); 1748 1749 unreg_image_encoder: 1750 pr_err("Cleanup: Destroy image encoder\n"); 1751 vchiq_mmal_component_finalise(dev->instance, 1752 dev->component[COMP_IMAGE_ENCODE]); 1753 1754 unreg_preview: 1755 pr_err("Cleanup: Destroy video render\n"); 1756 vchiq_mmal_component_finalise(dev->instance, 1757 dev->component[COMP_PREVIEW]); 1758 1759 unreg_camera: 1760 pr_err("Cleanup: Destroy camera\n"); 1761 vchiq_mmal_component_finalise(dev->instance, 1762 dev->component[COMP_CAMERA]); 1763 1764 unreg_mmal: 1765 vchiq_mmal_finalise(dev->instance); 1766 return ret; 1767 } 1768 1769 static int bcm2835_mmal_init_device(struct bcm2835_mmal_dev *dev, struct video_device *vfd) 1770 { 1771 int ret; 1772 1773 *vfd = vdev_template; 1774 1775 vfd->v4l2_dev = &dev->v4l2_dev; 1776 1777 vfd->lock = &dev->mutex; 1778 1779 vfd->queue = &dev->capture.vb_vidq; 1780 1781 /* video device needs to be able to access instance data */ 1782 video_set_drvdata(vfd, dev); 1783 1784 ret = video_register_device(vfd, VFL_TYPE_VIDEO, 1785 video_nr[dev->camera_num]); 1786 if (ret < 0) 1787 return ret; 1788 1789 v4l2_info(vfd->v4l2_dev, 1790 "V4L2 device registered as %s - stills mode > %dx%d\n", 1791 video_device_node_name(vfd), 1792 max_video_width, max_video_height); 1793 1794 return 0; 1795 } 1796 1797 static void bcm2835_cleanup_instance(struct bcm2835_mmal_dev *dev) 1798 { 1799 if (!dev) 1800 return; 1801 1802 v4l2_info(&dev->v4l2_dev, "unregistering %s\n", 1803 video_device_node_name(&dev->vdev)); 1804 1805 video_unregister_device(&dev->vdev); 1806 1807 if (dev->capture.encode_component) { 1808 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, 1809 "mmal_exit - disconnect tunnel\n"); 1810 vchiq_mmal_port_connect_tunnel(dev->instance, 1811 dev->capture.camera_port, NULL); 1812 vchiq_mmal_component_disable(dev->instance, 1813 dev->capture.encode_component); 1814 } 1815 vchiq_mmal_component_disable(dev->instance, 1816 dev->component[COMP_CAMERA]); 1817 1818 vchiq_mmal_component_finalise(dev->instance, 1819 dev->component[COMP_VIDEO_ENCODE]); 1820 1821 vchiq_mmal_component_finalise(dev->instance, 1822 dev->component[COMP_IMAGE_ENCODE]); 1823 1824 vchiq_mmal_component_finalise(dev->instance, 1825 dev->component[COMP_PREVIEW]); 1826 1827 vchiq_mmal_component_finalise(dev->instance, 1828 dev->component[COMP_CAMERA]); 1829 1830 v4l2_ctrl_handler_free(&dev->ctrl_handler); 1831 1832 v4l2_device_unregister(&dev->v4l2_dev); 1833 1834 kfree(dev); 1835 } 1836 1837 static struct v4l2_format default_v4l2_format = { 1838 .fmt.pix.pixelformat = V4L2_PIX_FMT_JPEG, 1839 .fmt.pix.width = 1024, 1840 .fmt.pix.bytesperline = 0, 1841 .fmt.pix.height = 768, 1842 .fmt.pix.sizeimage = 1024 * 768, 1843 }; 1844 1845 static int bcm2835_mmal_probe(struct vchiq_device *device) 1846 { 1847 int ret; 1848 struct bcm2835_mmal_dev *dev; 1849 struct vb2_queue *q; 1850 int camera; 1851 unsigned int num_cameras; 1852 struct vchiq_mmal_instance *instance; 1853 unsigned int resolutions[MAX_BCM2835_CAMERAS][2]; 1854 int i; 1855 1856 ret = dma_set_mask_and_coherent(&device->dev, DMA_BIT_MASK(32)); 1857 if (ret) { 1858 dev_err(&device->dev, "dma_set_mask_and_coherent failed: %d\n", ret); 1859 return ret; 1860 } 1861 1862 ret = vchiq_mmal_init(&instance); 1863 if (ret < 0) 1864 return ret; 1865 1866 num_cameras = get_num_cameras(instance, 1867 resolutions, 1868 MAX_BCM2835_CAMERAS); 1869 1870 if (num_cameras < 1) { 1871 ret = -ENODEV; 1872 goto cleanup_mmal; 1873 } 1874 1875 if (num_cameras > MAX_BCM2835_CAMERAS) 1876 num_cameras = MAX_BCM2835_CAMERAS; 1877 1878 for (camera = 0; camera < num_cameras; camera++) { 1879 dev = kzalloc(sizeof(*dev), GFP_KERNEL); 1880 if (!dev) { 1881 ret = -ENOMEM; 1882 goto cleanup_gdev; 1883 } 1884 1885 /* v4l2 core mutex used to protect all fops and v4l2 ioctls. */ 1886 mutex_init(&dev->mutex); 1887 dev->max_width = resolutions[camera][0]; 1888 dev->max_height = resolutions[camera][1]; 1889 1890 /* setup device defaults */ 1891 dev->overlay.w.left = 150; 1892 dev->overlay.w.top = 50; 1893 dev->overlay.w.width = 1024; 1894 dev->overlay.w.height = 768; 1895 dev->overlay.clipcount = 0; 1896 dev->overlay.field = V4L2_FIELD_NONE; 1897 dev->overlay.global_alpha = 255; 1898 1899 dev->capture.fmt = &formats[3]; /* JPEG */ 1900 1901 /* v4l device registration */ 1902 dev->camera_num = v4l2_device_set_name(&dev->v4l2_dev, KBUILD_MODNAME, 1903 &camera_instance); 1904 ret = v4l2_device_register(NULL, &dev->v4l2_dev); 1905 if (ret) { 1906 dev_err(&device->dev, "%s: could not register V4L2 device: %d\n", 1907 __func__, ret); 1908 goto free_dev; 1909 } 1910 1911 /* setup v4l controls */ 1912 ret = bcm2835_mmal_init_controls(dev, &dev->ctrl_handler); 1913 if (ret < 0) { 1914 v4l2_err(&dev->v4l2_dev, "%s: could not init controls: %d\n", 1915 __func__, ret); 1916 goto unreg_dev; 1917 } 1918 dev->v4l2_dev.ctrl_handler = &dev->ctrl_handler; 1919 1920 /* mmal init */ 1921 dev->instance = instance; 1922 ret = mmal_init(dev); 1923 if (ret < 0) { 1924 v4l2_err(&dev->v4l2_dev, "%s: mmal init failed: %d\n", 1925 __func__, ret); 1926 goto unreg_dev; 1927 } 1928 /* initialize queue */ 1929 q = &dev->capture.vb_vidq; 1930 memset(q, 0, sizeof(*q)); 1931 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 1932 q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ; 1933 q->drv_priv = dev; 1934 q->buf_struct_size = sizeof(struct vb2_mmal_buffer); 1935 q->ops = &bcm2835_mmal_video_qops; 1936 q->mem_ops = &vb2_vmalloc_memops; 1937 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 1938 q->lock = &dev->mutex; 1939 ret = vb2_queue_init(q); 1940 if (ret < 0) 1941 goto unreg_dev; 1942 1943 /* initialise video devices */ 1944 ret = bcm2835_mmal_init_device(dev, &dev->vdev); 1945 if (ret < 0) { 1946 v4l2_err(&dev->v4l2_dev, "%s: could not init device: %d\n", 1947 __func__, ret); 1948 goto unreg_dev; 1949 } 1950 1951 /* Really want to call vidioc_s_fmt_vid_cap with the default 1952 * format, but currently the APIs don't join up. 1953 */ 1954 ret = mmal_setup_components(dev, &default_v4l2_format); 1955 if (ret < 0) { 1956 v4l2_err(&dev->v4l2_dev, "%s: could not setup components: %d\n", 1957 __func__, ret); 1958 goto unreg_dev; 1959 } 1960 1961 v4l2_info(&dev->v4l2_dev, "Broadcom 2835 MMAL video capture loaded.\n"); 1962 1963 gdev[camera] = dev; 1964 } 1965 return 0; 1966 1967 unreg_dev: 1968 v4l2_ctrl_handler_free(&dev->ctrl_handler); 1969 v4l2_device_unregister(&dev->v4l2_dev); 1970 1971 free_dev: 1972 kfree(dev); 1973 1974 cleanup_gdev: 1975 for (i = 0; i < camera; i++) { 1976 bcm2835_cleanup_instance(gdev[i]); 1977 gdev[i] = NULL; 1978 } 1979 1980 cleanup_mmal: 1981 vchiq_mmal_finalise(instance); 1982 1983 return ret; 1984 } 1985 1986 static void bcm2835_mmal_remove(struct vchiq_device *device) 1987 { 1988 int camera; 1989 struct vchiq_mmal_instance *instance = gdev[0]->instance; 1990 1991 for (camera = 0; camera < MAX_BCM2835_CAMERAS; camera++) { 1992 bcm2835_cleanup_instance(gdev[camera]); 1993 gdev[camera] = NULL; 1994 } 1995 vchiq_mmal_finalise(instance); 1996 } 1997 1998 static const struct vchiq_device_id device_id_table[] = { 1999 { .name = "bcm2835-camera" }, 2000 {} 2001 }; 2002 MODULE_DEVICE_TABLE(vchiq, device_id_table); 2003 2004 static struct vchiq_driver bcm2835_camera_driver = { 2005 .probe = bcm2835_mmal_probe, 2006 .remove = bcm2835_mmal_remove, 2007 .id_table = device_id_table, 2008 .driver = { 2009 .name = "bcm2835-camera", 2010 }, 2011 }; 2012 2013 module_vchiq_driver(bcm2835_camera_driver) 2014 2015 MODULE_DESCRIPTION("Broadcom 2835 MMAL video capture"); 2016 MODULE_AUTHOR("Vincent Sanders"); 2017 MODULE_LICENSE("GPL"); 2018