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