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