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