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