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